Autoware.Auto
motion_common.hpp File Reference
#include <motion_common/visibility_control.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_auto_system_msgs/msg/control_diagnostic.hpp>
#include <autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <time_utils/time_utils.hpp>
#include <algorithm>
#include <cmath>
Include dependency graph for motion_common.hpp:
This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Namespaces

 motion
 
 motion::motion_common
 
 geometry_msgs
 
 geometry_msgs::msg
 

Typedefs

using motion::motion_common::Real = decltype(autoware_auto_planning_msgs::msg::TrajectoryPoint::longitudinal_velocity_mps)
 
using motion::motion_common::Command = autoware_auto_vehicle_msgs::msg::VehicleControlCommand
 
using motion::motion_common::Diagnostic = autoware_auto_system_msgs::msg::ControlDiagnostic
 
using motion::motion_common::State = autoware_auto_vehicle_msgs::msg::VehicleKinematicState
 
using motion::motion_common::Trajectory = autoware_auto_planning_msgs::msg::Trajectory
 
using motion::motion_common::Orientation = geometry_msgs::msg::Quaternion
 
using motion::motion_common::Double = decltype(Orientation::x)
 
using motion::motion_common::Index = decltype(Trajectory::points)::size_type
 
using motion::motion_common::Point = decltype(Trajectory::points)::value_type
 

Functions

MOTION_COMMON_PUBLIC bool motion::motion_common::is_past_point (const Point &state, const Point &pt) noexcept
 Check if a state is past a given trajectory point, assuming heading is correct. More...
 
MOTION_COMMON_PUBLIC bool motion::motion_common::is_past_point (const Point &state, const Point &current_pt, const Point &next_pt) noexcept
 
MOTION_COMMON_PUBLIC bool motion::motion_common::is_past_point (const Point &state, const Point &pt, Double nx, Double ny) noexcept
 Given a normal, determine if state is past a point. More...
 
template<typename IsPastPointF >
Index motion::motion_common::update_reference_index (const Trajectory &traj, const State &state, Index start_idx, IsPastPointF is_past_point)
 Advance to the first trajectory point past state according to criterion is_past_point. More...
 
MOTION_COMMON_PUBLIC bool motion::motion_common::heading_ok (const Trajectory &traj)
 Check that all heading values in a trajectory are normalized 2D quaternions. More...
 
MOTION_COMMON_PUBLIC void motion::motion_common::doTransform (const Point &t_in, Point &t_out, const geometry_msgs::msg::TransformStamped &transform) noexcept
 Apply transform to TrajectoryPoint. More...
 
MOTION_COMMON_PUBLIC void motion::motion_common::doTransform (const State &t_in, State &t_out, const geometry_msgs::msg::TransformStamped &transform) noexcept
 Apply transform to VehicleKinematicState. More...
 
MOTION_COMMON_PUBLIC Double motion::motion_common::to_angle (Orientation orientation) noexcept
 Converts 3D quaternion to simple heading representation. More...
 
template<typename RealT >
Orientation motion::motion_common::from_angles (RealT roll, RealT pitch, RealT yaw) noexcept
 Converts angles into a corresponding Orientation. More...
 
template<typename RealT >
Orientation motion::motion_common::from_angle (RealT angle) noexcept
 Converts a heading angle into a corresponding Orientation. More...
 
template<typename T >
motion::motion_common::clamp (T val, T min, T max)
 Standard clamp implementation. More...
 
template<typename T , typename RealT = double>
motion::motion_common::interpolate (T a, T b, RealT t)
 Standard linear interpolation. More...
 
MOTION_COMMON_PUBLIC Orientation motion::motion_common::slerp (const Orientation &a, const Orientation &b, const Real t)
 Spherical linear interpolation. More...
 
template<typename SlerpF >
Point motion::motion_common::interpolate (Point a, Point b, Real t, SlerpF slerp_fn)
 Trajectory point interpolation. More...
 
MOTION_COMMON_PUBLIC Point motion::motion_common::interpolate (Point a, Point b, Real t)
 Default point interpolation. More...
 
template<typename SlerpF >
void motion::motion_common::sample (const Trajectory &in, Trajectory &out, std::chrono::nanoseconds period, SlerpF slerp_fn)
 Sample a trajectory using interpolation; does not extrapolate. More...
 
MOTION_COMMON_PUBLIC void motion::motion_common::sample (const Trajectory &in, Trajectory &out, std::chrono::nanoseconds period)
 Trajectory sampling with default interpolation. More...
 
MOTION_COMMON_PUBLIC void motion::motion_common::error (const Point &state, const Point &ref, Diagnostic &out) noexcept
 Diagnostic header stuff. More...
 
MOTION_COMMON_PUBLIC Quaternion geometry_msgs::msg::operator+ (Quaternion a, Quaternion b) noexcept
 Addition operator. More...
 
MOTION_COMMON_PUBLIC Quaternion geometry_msgs::msg::operator- (Quaternion a) noexcept
 Unary minus. More...
 
MOTION_COMMON_PUBLIC Quaternion geometry_msgs::msg::operator- (Quaternion a, Quaternion b) noexcept
 Difference operator. More...