Autoware.Auto
mpc_utils.hpp
Go to the documentation of this file.
1 // Copyright 2018-2021 The Autoware Foundation
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 //    http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_
16 #define TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_
17 
18 #include <cmath>
19 #include <string>
20 #include <vector>
21 
25 
26 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
27 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
28 #include "common/types.hpp"
29 #include "eigen3/Eigen/Core"
30 #include "geometry/common_2d.hpp"
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"
38 
39 
40 namespace autoware
41 {
42 namespace motion
43 {
44 namespace control
45 {
46 namespace trajectory_follower
47 {
48 namespace MPCUtils
49 {
57 TRAJECTORY_FOLLOWER_PUBLIC geometry_msgs::msg::Quaternion getQuaternionFromYaw(
58  const float64_t & yaw);
63 TRAJECTORY_FOLLOWER_PUBLIC void convertEulerAngleToMonotonic(std::vector<float64_t> * a);
70 TRAJECTORY_FOLLOWER_PUBLIC float64_t calcLateralError(
71  const geometry_msgs::msg::Pose & ego_pose, const geometry_msgs::msg::Pose & ref_pose);
78 TRAJECTORY_FOLLOWER_PUBLIC bool8_t convertToMPCTrajectory(
86 TRAJECTORY_FOLLOWER_PUBLIC bool8_t convertToAutowareTrajectory(
93 TRAJECTORY_FOLLOWER_PUBLIC void calcMPCTrajectoryArclength(
94  const MPCTrajectory & trajectory, std::vector<float64_t> * arclength);
101 TRAJECTORY_FOLLOWER_PUBLIC bool8_t resampleMPCTrajectoryByDistance(
102  const MPCTrajectory & input, const float64_t resample_interval_dist, MPCTrajectory * output);
110 TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpMPCTrajectory(
111  const std::vector<float64_t> & in_index, const MPCTrajectory & in_traj,
112  const std::vector<float64_t> & out_index, MPCTrajectory * out_traj);
118 TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcMPCTrajectoryTime(MPCTrajectory & traj);
127 TRAJECTORY_FOLLOWER_PUBLIC void dynamicSmoothingVelocity(
128  const size_t start_idx, const float64_t start_vel, const float64_t acc_lim, const float64_t tau,
129  MPCTrajectory & traj);
136 TRAJECTORY_FOLLOWER_PUBLIC void calcTrajectoryYawFromXY(
137  MPCTrajectory * traj, const int64_t nearest_idx, const float64_t ego_yaw);
143 TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcTrajectoryCurvature(
144  const size_t curvature_smoothing_num,
145  MPCTrajectory * traj);
152 TRAJECTORY_FOLLOWER_PUBLIC std::vector<float64_t> calcTrajectoryCurvature(
153  const size_t curvature_smoothing_num, const MPCTrajectory & traj);
165 TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcNearestPoseInterp(
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);
175 TRAJECTORY_FOLLOWER_PUBLIC int64_t calcNearestIndex(
176  const MPCTrajectory & traj,
177  const geometry_msgs::msg::Pose & self_pose);
184 TRAJECTORY_FOLLOWER_PUBLIC int64_t calcNearestIndex(
186  const geometry_msgs::msg::Pose & self_pose);
190 TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance(
191  const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory,
192  const int64_t origin);
193 } // namespace MPCUtils
194 } // namespace trajectory_follower
195 } // namespace control
196 } // namespace motion
197 } // namespace autoware
198 #endif // TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_
output
Definition: output_type_trait.hpp:32
visibility_control.hpp
autoware::motion::control::trajectory_follower::MPCUtils::getQuaternionFromYaw
TRAJECTORY_FOLLOWER_PUBLIC geometry_msgs::msg::Quaternion getQuaternionFromYaw(const float64_t &yaw)
convert from yaw to ros-Quaternion
Definition: mpc_utils.cpp:34
autoware::motion::control::trajectory_follower::MPCUtils::resampleMPCTrajectoryByDistance
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
types.hpp
This file includes common type definition.
autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory
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
autoware::motion::control::trajectory_follower::MPCUtils::calcStopDistance
TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance(const autoware_auto_planning_msgs::msg::Trajectory &current_trajectory, const int64_t origin)
calculate distance to stopped point
Definition: mpc_utils.cpp:456
common_2d.hpp
This file includes common functionality for 2D geometry, such as dot products.
autoware::motion::control::trajectory_follower::MPCUtils::calcNearestIndex
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
angle_utils.hpp
autoware::motion::control::trajectory_follower::MPCUtils::calcTrajectoryYawFromXY
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
autoware::motion::control::trajectory_follower::MPCUtils::calcLateralError
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
interpolate.hpp
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
autoware::motion::control::trajectory_follower::MPCUtils::calcMPCTrajectoryTime
TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcMPCTrajectoryTime(MPCTrajectory &traj)
fill the relative_time field of the given MPCTrajectory
Definition: mpc_utils.cpp:283
autoware::motion::control::trajectory_follower::MPCUtils::calcNearestPoseInterp
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
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::motion::control::trajectory_follower::MPCUtils::calcTrajectoryCurvature
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
autoware::motion::control::trajectory_follower::MPCUtils::linearInterpMPCTrajectory
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
motion_common.hpp
mpc_trajectory.hpp
autoware::motion::control::trajectory_follower
Definition: debug_values.hpp:29
autoware::motion::control::trajectory_follower::MPCUtils::convertEulerAngleToMonotonic
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
motion
Definition: controller_base.hpp:31
motion::motion_common::Trajectory
autoware_auto_planning_msgs::msg::Trajectory Trajectory
Definition: motion_common.hpp:41
autoware::motion::control::trajectory_follower::MPCUtils::dynamicSmoothingVelocity
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
yaw
DifferentialState yaw
Definition: kinematic_bicycle.snippet.hpp:28
autoware::common::types::float64_t
double float64_t
Definition: types.hpp:47
autoware::motion::control::trajectory_follower::MPCUtils::convertToMPCTrajectory
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
autoware::motion::control::trajectory_follower::MPCUtils::calcMPCTrajectoryArclength
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
autoware::motion::control::trajectory_follower::MPCTrajectory
calculate control command to follow reference waypoints
Definition: mpc_trajectory.hpp:37