Autoware.Auto
trajectory_common.hpp File Reference
#include <experimental/optional>
#include <limits>
#include <stdexcept>
#include <vector>
#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.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "helper_functions/angle_utils.hpp"
#include "motion_common/motion_common.hpp"
#include "tf2/utils.h"
Include dependency graph for trajectory_common.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::motion_common
 

Typedefs

typedef autoware_auto_planning_msgs::msg::TrajectoryPoint autoware::motion::motion_common::Point
 
typedef Eigen::Matrix< float64_t, 3, 1 > autoware::motion::motion_common::Vector3f
 

Functions

MOTION_COMMON_PUBLIC void autoware::motion::motion_common::validateNonEmpty (const Points &points)
 throws an exception if the given list of points is empty More...
 
MOTION_COMMON_PUBLIC float64_t autoware::motion::motion_common::calcYawDeviation (const float64_t &base_yaw, const float64_t &target_yaw)
 calculate the yaw deviation between two angles More...
 
MOTION_COMMON_PUBLIC std::experimental::optional< size_t > autoware::motion::motion_common::searchZeroVelocityIndex (const Points &points, const size_t src_idx, const size_t dst_idx, const float64_t epsilon=1e-3)
 search first index with a velocity of zero in the given range of points More...
 
MOTION_COMMON_PUBLIC std::experimental::optional< size_t > autoware::motion::motion_common::searchZeroVelocityIndex (const Points &points)
 search first index with a velocity of zero in the given points More...
 
MOTION_COMMON_PUBLIC size_t autoware::motion::motion_common::findNearestIndex (const Points &points, const geometry_msgs::msg::Point &point)
 search the index of the point nearest to the given target More...
 
MOTION_COMMON_PUBLIC std::experimental::optional< size_t > autoware::motion::motion_common::findNearestIndex (const Points &points, const geometry_msgs::msg::Pose &pose, const float64_t max_dist=std::numeric_limits< float64_t >::max(), const float64_t max_yaw=std::numeric_limits< float64_t >::max())
 search the index of the point nearest to the given target with limits on the distance and yaw deviation More...
 
MOTION_COMMON_PUBLIC float64_t autoware::motion::motion_common::calcLongitudinalOffsetToSegment (const Points &points, const size_t seg_idx, const geometry_msgs::msg::Point &p_target)
 calculate length along trajectory from seg_idx point to nearest point to p_target on trajectory If seg_idx point is after that nearest point, length is negative More...
 
MOTION_COMMON_PUBLIC size_t autoware::motion::motion_common::findNearestSegmentIndex (const Points &points, const geometry_msgs::msg::Point &point)
 find nearest segment index to point segment is straight path between two continuous points of trajectory When point is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 More...
 
MOTION_COMMON_PUBLIC float64_t autoware::motion::motion_common::calcSignedArcLength (const Points &points, const size_t src_idx, const size_t dst_idx)
 calculate arc length along points More...
 
MOTION_COMMON_PUBLIC float64_t autoware::motion::motion_common::calcSignedArcLength (const Points &points, const geometry_msgs::msg::Point &src_point, const size_t &dst_idx)
 calculate arc length along points More...
 
MOTION_COMMON_PUBLIC float64_t autoware::motion::motion_common::calcSignedArcLength (const Points &points, const geometry_msgs::msg::Point &src_point, const geometry_msgs::msg::Point &dst_point)
 calculate arc length along points More...
 
MOTION_COMMON_PUBLIC float64_t autoware::motion::motion_common::calcLongitudinalDeviation (const geometry_msgs::msg::Pose &base_pose, const geometry_msgs::msg::Point &target_point)
 calculate longitudinal deviation of a point relative to a pose More...
 

Variables

decltype(autoware_auto_planning_msgs::msg::Trajectory::points) typedef autoware::motion::motion_common::Points