Autoware.Auto
longitudinal_controller_utils.hpp File Reference
#include <cmath>
#include <experimental/optional>
#include <limits>
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Geometry"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "common/types.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry/common_2d.hpp"
#include "motion_common/motion_common.hpp"
#include "motion_common/trajectory_common.hpp"
#include "tf2/utils.h"
#include "trajectory_follower/visibility_control.hpp"
Include dependency graph for longitudinal_controller_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::longitudinal_utils
 

Functions

TRAJECTORY_FOLLOWER_PUBLIC bool8_t autoware::motion::control::trajectory_follower::longitudinal_utils::isValidTrajectory (const Trajectory &traj)
 check if trajectory is invalid or not More...
 
TRAJECTORY_FOLLOWER_PUBLIC 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 More...
 
TRAJECTORY_FOLLOWER_PUBLIC float64_t autoware::motion::control::trajectory_follower::longitudinal_utils::getPitchByPose (const Quaternion &quaternion)
 calculate pitch angle from estimated current pose More...
 
TRAJECTORY_FOLLOWER_PUBLIC 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 More...
 
TRAJECTORY_FOLLOWER_PUBLIC float64_t autoware::motion::control::trajectory_follower::longitudinal_utils::calcElevationAngle (const TrajectoryPoint &p_from, const TrajectoryPoint &p_to)
 calculate elevation angle More...
 
TRAJECTORY_FOLLOWER_PUBLIC 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 More...
 
TRAJECTORY_FOLLOWER_PUBLIC 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 More...
 
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 More...
 
TRAJECTORY_FOLLOWER_PUBLIC 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 More...
 
TRAJECTORY_FOLLOWER_PUBLIC 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 More...