Go to the documentation of this file.
15 #ifndef TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_
16 #define TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_
26 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
27 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
29 #include "eigen3/Eigen/Core"
31 #include "geometry_msgs/msg/pose_stamped.hpp"
32 #include "geometry_msgs/msg/twist_stamped.hpp"
35 #include "rclcpp/rclcpp.hpp"
36 #include "tf2/utils.h"
37 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
71 const geometry_msgs::msg::Pose & ego_pose,
const geometry_msgs::msg::Pose & ref_pose);
94 const MPCTrajectory & trajectory, std::vector<float64_t> * arclength);
111 const std::vector<float64_t> & in_index,
const MPCTrajectory & in_traj,
112 const std::vector<float64_t> & out_index,
MPCTrajectory * out_traj);
144 const size_t curvature_smoothing_num,
153 const size_t curvature_smoothing_num,
const MPCTrajectory & traj);
166 const MPCTrajectory & traj,
const geometry_msgs::msg::Pose & self_pose,
167 geometry_msgs::msg::Pose * nearest_pose,
size_t * nearest_index,
float64_t * nearest_time,
168 const rclcpp::Logger & logger, rclcpp::Clock & clock);
177 const geometry_msgs::msg::Pose & self_pose);
186 const geometry_msgs::msg::Pose & self_pose);
192 const int64_t origin);
198 #endif // TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_
Definition: output_type_trait.hpp:32
TRAJECTORY_FOLLOWER_PUBLIC geometry_msgs::msg::Quaternion getQuaternionFromYaw(const float64_t &yaw)
convert from yaw to ros-Quaternion
Definition: mpc_utils.cpp:34
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
Definition: mpc_utils.cpp:76
This file includes common type definition.
TRAJECTORY_FOLLOWER_PUBLIC bool8_t convertToAutowareTrajectory(const MPCTrajectory &input, autoware_auto_planning_msgs::msg::Trajectory &output)
convert the given MPCTrajectory to a Trajectory msg
Definition: mpc_utils.cpp:265
TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance(const autoware_auto_planning_msgs::msg::Trajectory ¤t_trajectory, const int64_t origin)
calculate distance to stopped point
Definition: mpc_utils.cpp:456
This file includes common functionality for 2D geometry, such as dot products.
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
Definition: mpc_utils.cpp:321
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
Definition: mpc_utils.cpp:160
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
Definition: mpc_utils.cpp:52
bool bool8_t
Definition: types.hpp:39
TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcMPCTrajectoryTime(MPCTrajectory &traj)
fill the relative_time field of the given MPCTrajectory
Definition: mpc_utils.cpp:283
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
Definition: mpc_utils.cpp:380
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
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...
Definition: mpc_utils.cpp:196
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
Definition: mpc_utils.cpp:121
Definition: debug_values.hpp:29
TRAJECTORY_FOLLOWER_PUBLIC void convertEulerAngleToMonotonic(std::vector< float64_t > *a)
convert Euler angle vector including +-2pi to 0 jump to continuous series data
Definition: mpc_utils.cpp:41
Definition: controller_base.hpp:31
autoware_auto_planning_msgs::msg::Trajectory Trajectory
Definition: motion_common.hpp:41
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
Definition: mpc_utils.cpp:300
DifferentialState yaw
Definition: kinematic_bicycle.snippet.hpp:28
double float64_t
Definition: types.hpp:47
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
Definition: mpc_utils.cpp:247
TRAJECTORY_FOLLOWER_PUBLIC void calcMPCTrajectoryArclength(const MPCTrajectory &trajectory, std::vector< float64_t > *arclength)
calculate the arc length at each point of the given trajectory
Definition: mpc_utils.cpp:62
calculate control command to follow reference waypoints
Definition: mpc_trajectory.hpp:37