|
|
@@ -16,9 +16,9 @@
|
|
|
|
|
|
#include "interpolation/linear_interpolation.hpp"
|
|
|
#include "interpolation/spline_interpolation.hpp"
|
|
|
-#include "motion_utils/trajectory/trajectory.hpp"
|
|
|
-#include "tier4_autoware_utils/geometry/geometry.hpp"
|
|
|
-#include "tier4_autoware_utils/math/normalization.hpp"
|
|
|
+//#include "motion_utils/trajectory/trajectory.hpp"
|
|
|
+//#include "tier4_autoware_utils/geometry/geometry.hpp"
|
|
|
+//#include "tier4_autoware_utils/math/normalization.hpp"
|
|
|
|
|
|
#include <algorithm>
|
|
|
#include <limits>
|
|
|
@@ -30,8 +30,8 @@ namespace autoware::motion::control::mpc_lateral_controller
|
|
|
namespace
|
|
|
{
|
|
|
double calcLongitudinalOffset(
|
|
|
- const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back,
|
|
|
- const geometry_msgs::msg::Point & p_target)
|
|
|
+ const iv::ADCPoint & p_front, const iv::ADCPoint & p_back,
|
|
|
+ const iv::ADCPoint & p_target)
|
|
|
{
|
|
|
const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0};
|
|
|
const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0};
|
|
|
@@ -47,7 +47,7 @@ namespace MPCUtils
|
|
|
//using tier4_autoware_utils::normalizeRadian;
|
|
|
|
|
|
|
|
|
-double ADCgetYaw(iv::Quaternion_ q)
|
|
|
+double ADCgetYaw(iv::ADCQuaternion q)
|
|
|
{
|
|
|
// yaw (z-axis rotation)
|
|
|
double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
|
|
|
@@ -57,6 +57,231 @@ double ADCgetYaw(iv::Quaternion_ q)
|
|
|
return yaw;
|
|
|
}
|
|
|
|
|
|
+iv::ADCQuaternion ADCcreateQuaternionFromYaw(double yaw) // yaw (Z), pitch (Y), roll (X)
|
|
|
+{
|
|
|
+ double pitch = 0;
|
|
|
+ double roll = 0;
|
|
|
+ // Abbreviations for the various angular functions
|
|
|
+ double cy = cos(yaw * 0.5);
|
|
|
+ double sy = sin(yaw * 0.5);
|
|
|
+ double cp = cos(pitch * 0.5);
|
|
|
+ double sp = sin(pitch * 0.5);
|
|
|
+ double cr = cos(roll * 0.5);
|
|
|
+ double sr = sin(roll * 0.5);
|
|
|
+
|
|
|
+ iv::ADCQuaternion q;
|
|
|
+ q.w = cy * cp * cr + sy * sp * sr;
|
|
|
+ q.x = cy * cp * sr - sy * sp * cr;
|
|
|
+ q.y = sy * cp * sr + cy * sp * cr;
|
|
|
+ q.z = sy * cp * cr - cy * sp * sr;
|
|
|
+
|
|
|
+ return q;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+inline static double ADCnormalizeDegree(const double deg, const double min_deg = -180)
|
|
|
+{
|
|
|
+ const auto max_deg = min_deg + 360.0;
|
|
|
+
|
|
|
+ const auto value = std::fmod(deg, 360.0);
|
|
|
+ if (min_deg <= value && value < max_deg) {
|
|
|
+ return value;
|
|
|
+ }
|
|
|
+
|
|
|
+ return value - std::copysign(360.0, value);
|
|
|
+}
|
|
|
+
|
|
|
+inline double ADCnormalizeRadian(const double rad, const double min_rad = -M_PI)
|
|
|
+{
|
|
|
+ const auto max_rad = min_rad + 2 * M_PI;
|
|
|
+
|
|
|
+ const auto value = std::fmod(rad, 2 * M_PI);
|
|
|
+ if (min_rad <= value && value < max_rad) {
|
|
|
+ return value;
|
|
|
+ }
|
|
|
+
|
|
|
+ return value - std::copysign(2 * M_PI, value);
|
|
|
+}
|
|
|
+
|
|
|
+inline double ADCcalcDistance2d(iv::ADCPoint p1, iv::ADCPoint p2)
|
|
|
+{
|
|
|
+ return sqrt(pow(p2.x - p1.x,2)+pow(p2.y - p1.y,2));
|
|
|
+}
|
|
|
+
|
|
|
+inline static double ADCcalcCurvature(
|
|
|
+ const iv::ADCPoint & p1, const iv::ADCPoint & p2,
|
|
|
+ const iv::ADCPoint & p3)
|
|
|
+{
|
|
|
+ // Calculation details are described in the following page
|
|
|
+ // https://en.wikipedia.org/wiki/Menger_curvature
|
|
|
+ const double denominator =
|
|
|
+ ADCcalcDistance2d(p1, p2) * ADCcalcDistance2d(p2, p3) * ADCcalcDistance2d(p3, p1);
|
|
|
+ if (std::fabs(denominator) < 1e-10) {
|
|
|
+ throw std::runtime_error("points are too close for curvature calculation.");
|
|
|
+ }
|
|
|
+ return 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / denominator;
|
|
|
+}
|
|
|
+
|
|
|
+double ADCcalcLateralDeviation(
|
|
|
+ const iv::ADCPose & base_pose, const iv::ADCPoint & target_point)
|
|
|
+{
|
|
|
+ const auto & base_point = base_pose.position;
|
|
|
+
|
|
|
+ const auto yaw = ADCgetYaw(base_pose.orientation);
|
|
|
+ const Eigen::Vector3d base_unit_vec{std::cos(yaw), std::sin(yaw), 0};
|
|
|
+
|
|
|
+ const auto dx = target_point.x - base_point.x;
|
|
|
+ const auto dy = target_point.y - base_point.y;
|
|
|
+ const Eigen::Vector3d diff_vec{dx, dy, 0};
|
|
|
+
|
|
|
+ const Eigen::Vector3d cross_vec = base_unit_vec.cross(diff_vec);
|
|
|
+
|
|
|
+ return cross_vec.z();
|
|
|
+}
|
|
|
+
|
|
|
+double ADCcalcLongitudinalDeviation(
|
|
|
+ const iv::ADCPose & base_pose, const iv::ADCPoint & target_point)
|
|
|
+{
|
|
|
+ const auto & base_point = base_pose.position;
|
|
|
+
|
|
|
+ const auto yaw = ADCgetYaw(base_pose.orientation);
|
|
|
+ const Eigen::Vector3d base_unit_vec{std::cos(yaw), std::sin(yaw), 0};
|
|
|
+
|
|
|
+ const auto dx = target_point.x - base_point.x;
|
|
|
+ const auto dy = target_point.y - base_point.y;
|
|
|
+ const Eigen::Vector3d diff_vec{dx, dy, 0};
|
|
|
+
|
|
|
+ return base_unit_vec.dot(diff_vec);
|
|
|
+}
|
|
|
+
|
|
|
+double ADCcalcYawDeviation(
|
|
|
+ const iv::ADCPose & base_pose, const iv::ADCPose & target_pose)
|
|
|
+{
|
|
|
+ const auto base_yaw = ADCgetYaw(base_pose.orientation);
|
|
|
+ const auto target_yaw = ADCgetYaw(target_pose.orientation);
|
|
|
+ return ADCnormalizeRadian(target_yaw - base_yaw);
|
|
|
+}
|
|
|
+
|
|
|
+double ADCcalcYawDeviation(
|
|
|
+ const double & base_yaw, const iv::ADCPose & target_pose)
|
|
|
+{
|
|
|
+ const auto target_yaw = ADCgetYaw(target_pose.orientation);
|
|
|
+ return ADCnormalizeRadian(target_yaw - base_yaw);
|
|
|
+}
|
|
|
+
|
|
|
+double calcSquaredDistance2d(const iv::ADCPoint & point1, const iv::ADCPoint & point2)
|
|
|
+{
|
|
|
+ const auto dx = point1.x - point2.x;
|
|
|
+ const auto dy = point1.y - point2.y;
|
|
|
+ return dx * dx + dy * dy;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+size_t findNearestIndex(const MPCTrajectory & currenttraj, const iv::ADCPoint & point)
|
|
|
+{
|
|
|
+
|
|
|
+ double min_dist = std::numeric_limits<double>::max();
|
|
|
+ size_t min_idx = 0;
|
|
|
+
|
|
|
+ for (size_t i = 0; i < currenttraj.size(); ++i) {
|
|
|
+ const auto dist = pow(currenttraj.x[i] - point.x,2) + pow(currenttraj.y[i] - point.y,2);
|
|
|
+ if (dist < min_dist) {
|
|
|
+ min_dist = dist;
|
|
|
+ min_idx = i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return min_idx;
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief find first nearest point index through points container for a given pose with soft
|
|
|
+ * distance and yaw constraints. Finding nearest point is determined by looping through the points
|
|
|
+ * container, and finding the nearest point to the given pose in terms of squared 2D distance and
|
|
|
+ * yaw deviation. The index of the point with minimum distance and yaw deviation comparing to the
|
|
|
+ * given pose will be returned.
|
|
|
+ * @param points points of trajectory, path, ...
|
|
|
+ * @param pose given pose
|
|
|
+ * @param dist_threshold distance threshold used for searching for first nearest index to given pose
|
|
|
+ * @param yaw_threshold yaw threshold used for searching for first nearest index to given pose
|
|
|
+ * @return index of nearest point (index or none if not found)
|
|
|
+ */
|
|
|
+static size_t findFirstNearestIndexWithSoftConstraints(
|
|
|
+ const MPCTrajectory & currenttraj, const iv::ADCPose & pose,
|
|
|
+ const double dist_threshold = std::numeric_limits<double>::max(),
|
|
|
+ const double yaw_threshold = std::numeric_limits<double>::max())
|
|
|
+{
|
|
|
+
|
|
|
+ { // with dist and yaw thresholds
|
|
|
+ const double squared_dist_threshold = dist_threshold * dist_threshold;
|
|
|
+ double min_squared_dist = std::numeric_limits<double>::max();
|
|
|
+ size_t min_idx = 0;
|
|
|
+ bool is_within_constraints = false;
|
|
|
+ for (size_t i = 0; i < currenttraj.size(); ++i) {
|
|
|
+ const auto squared_dist = pow(pose.position.x - currenttraj.x[i],2) + pow(pose.position.y - currenttraj.y[i],2);
|
|
|
+ // tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose.position);
|
|
|
+ const auto yaw =
|
|
|
+ ADCcalcYawDeviation(currenttraj.yaw[i], pose);
|
|
|
+
|
|
|
+ if (squared_dist_threshold < squared_dist || yaw_threshold < std::abs(yaw)) {
|
|
|
+ if (is_within_constraints) {
|
|
|
+ break;
|
|
|
+ } else {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (min_squared_dist <= squared_dist) {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ min_squared_dist = squared_dist;
|
|
|
+ min_idx = i;
|
|
|
+ is_within_constraints = true;
|
|
|
+ }
|
|
|
+
|
|
|
+ // nearest index is found
|
|
|
+ if (is_within_constraints) {
|
|
|
+ return min_idx;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ { // with dist threshold
|
|
|
+ const double squared_dist_threshold = dist_threshold * dist_threshold;
|
|
|
+ double min_squared_dist = std::numeric_limits<double>::max();
|
|
|
+ size_t min_idx = 0;
|
|
|
+ bool is_within_constraints = false;
|
|
|
+ for (size_t i = 0; i < currenttraj.size(); ++i) {
|
|
|
+ const auto squared_dist = pow(pose.position.x - currenttraj.x[i],2) + pow(pose.position.y - currenttraj.y[i],2);
|
|
|
+ // tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose.position);
|
|
|
+
|
|
|
+ if (squared_dist_threshold < squared_dist) {
|
|
|
+ if (is_within_constraints) {
|
|
|
+ break;
|
|
|
+ } else {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (min_squared_dist <= squared_dist) {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ min_squared_dist = squared_dist;
|
|
|
+ min_idx = i;
|
|
|
+ is_within_constraints = true;
|
|
|
+ }
|
|
|
+
|
|
|
+ // nearest index is found
|
|
|
+ if (is_within_constraints) {
|
|
|
+ return min_idx;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // without any threshold
|
|
|
+ return findNearestIndex(currenttraj, pose.position);
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
double calcDistance2d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2)
|
|
|
{
|
|
|
const double dx = trajectory.x.at(idx1) - trajectory.x.at(idx2);
|
|
|
@@ -76,7 +301,7 @@ void convertEulerAngleToMonotonic(std::vector<double> & angle_vector)
|
|
|
{
|
|
|
for (uint i = 1; i < angle_vector.size(); ++i) {
|
|
|
const double da = angle_vector.at(i) - angle_vector.at(i - 1);
|
|
|
- angle_vector.at(i) = angle_vector.at(i - 1) + normalizeRadian(da);
|
|
|
+ angle_vector.at(i) = angle_vector.at(i - 1) + ADCnormalizeRadian(da);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
@@ -206,7 +431,8 @@ void calcTrajectoryYawFromXY(MPCTrajectory & traj, const bool is_forward_shift)
|
|
|
return;
|
|
|
}
|
|
|
if (traj.yaw.size() != traj.vx.size()) {
|
|
|
- RCLCPP_ERROR(rclcpp::get_logger("mpc_utils"), "trajectory size has no consistency.");
|
|
|
+ std::cout<<"trajectory size has no consistency."<<std::endl;
|
|
|
+ // RCLCPP_ERROR(rclcpp::get_logger("mpc_utils"), "trajectory size has no consistency.");
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
@@ -236,7 +462,7 @@ std::vector<double> calcTrajectoryCurvature(
|
|
|
std::vector<double> curvature_vec(traj.x.size());
|
|
|
|
|
|
/* calculate curvature by circle fitting from three points */
|
|
|
- geometry_msgs::msg::Point p1, p2, p3;
|
|
|
+ iv::ADCPoint p1, p2, p3;
|
|
|
const int max_smoothing_num =
|
|
|
static_cast<int>(std::floor(0.5 * (static_cast<double>(traj.x.size() - 1))));
|
|
|
const size_t L = static_cast<size_t>(std::min(curvature_smoothing_num, max_smoothing_num));
|
|
|
@@ -251,7 +477,7 @@ std::vector<double> calcTrajectoryCurvature(
|
|
|
p2.y = traj.y.at(curr_idx);
|
|
|
p3.y = traj.y.at(next_idx);
|
|
|
try {
|
|
|
- curvature_vec.at(curr_idx) = tier4_autoware_utils::calcCurvature(p1, p2, p3);
|
|
|
+ curvature_vec.at(curr_idx) = ADCcalcCurvature(p1, p2, p3);
|
|
|
} catch (...) {
|
|
|
std::cerr << "[MPC] 2 points are too close to calculate curvature." << std::endl;
|
|
|
curvature_vec.at(curr_idx) = 0.0;
|
|
|
@@ -267,41 +493,41 @@ std::vector<double> calcTrajectoryCurvature(
|
|
|
return curvature_vec;
|
|
|
}
|
|
|
|
|
|
-MPCTrajectory convertToMPCTrajectory(const Trajectory & input)
|
|
|
-{
|
|
|
- MPCTrajectory output;
|
|
|
- for (const TrajectoryPoint & p : input.points) {
|
|
|
- const double x = p.pose.position.x;
|
|
|
- const double y = p.pose.position.y;
|
|
|
- const double z = p.pose.position.z;
|
|
|
- const double yaw = tf2::getYaw(p.pose.orientation);
|
|
|
- const double vx = p.longitudinal_velocity_mps;
|
|
|
- const double k = 0.0;
|
|
|
- const double t = 0.0;
|
|
|
- output.push_back(x, y, z, yaw, vx, k, k, t);
|
|
|
- }
|
|
|
- calcMPCTrajectoryTime(output);
|
|
|
- return output;
|
|
|
-}
|
|
|
+//MPCTrajectory convertToMPCTrajectory(const Trajectory & input)
|
|
|
+//{
|
|
|
+// MPCTrajectory output;
|
|
|
+// for (const TrajectoryPoint & p : input.points) {
|
|
|
+// const double x = p.pose.position.x;
|
|
|
+// const double y = p.pose.position.y;
|
|
|
+// const double z = p.pose.position.z;
|
|
|
+// const double yaw = tf2::getYaw(p.pose.orientation);
|
|
|
+// const double vx = p.longitudinal_velocity_mps;
|
|
|
+// const double k = 0.0;
|
|
|
+// const double t = 0.0;
|
|
|
+// output.push_back(x, y, z, yaw, vx, k, k, t);
|
|
|
+// }
|
|
|
+// calcMPCTrajectoryTime(output);
|
|
|
+// return output;
|
|
|
+//}
|
|
|
|
|
|
-Trajectory convertToAutowareTrajectory(const MPCTrajectory & input)
|
|
|
-{
|
|
|
- Trajectory output;
|
|
|
- TrajectoryPoint p;
|
|
|
- for (size_t i = 0; i < input.size(); ++i) {
|
|
|
- p.pose.position.x = input.x.at(i);
|
|
|
- p.pose.position.y = input.y.at(i);
|
|
|
- p.pose.position.z = input.z.at(i);
|
|
|
- p.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(input.yaw.at(i));
|
|
|
- p.longitudinal_velocity_mps =
|
|
|
- static_cast<decltype(p.longitudinal_velocity_mps)>(input.vx.at(i));
|
|
|
- output.points.push_back(p);
|
|
|
- if (output.points.size() == output.points.max_size()) {
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
- return output;
|
|
|
-}
|
|
|
+//Trajectory convertToAutowareTrajectory(const MPCTrajectory & input)
|
|
|
+//{
|
|
|
+// Trajectory output;
|
|
|
+// TrajectoryPoint p;
|
|
|
+// for (size_t i = 0; i < input.size(); ++i) {
|
|
|
+// p.pose.position.x = input.x.at(i);
|
|
|
+// p.pose.position.y = input.y.at(i);
|
|
|
+// p.pose.position.z = input.z.at(i);
|
|
|
+// p.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(input.yaw.at(i));
|
|
|
+// p.longitudinal_velocity_mps =
|
|
|
+// static_cast<decltype(p.longitudinal_velocity_mps)>(input.vx.at(i));
|
|
|
+// output.points.push_back(p);
|
|
|
+// if (output.points.size() == output.points.max_size()) {
|
|
|
+// break;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// return output;
|
|
|
+//}
|
|
|
|
|
|
bool calcMPCTrajectoryTime(MPCTrajectory & traj)
|
|
|
{
|
|
|
@@ -342,29 +568,30 @@ void dynamicSmoothingVelocity(
|
|
|
}
|
|
|
|
|
|
bool calcNearestPoseInterp(
|
|
|
- const MPCTrajectory & traj, const Pose & self_pose, Pose * nearest_pose, size_t * nearest_index,
|
|
|
+ const MPCTrajectory & traj, const iv::ADCPose & self_pose, iv::ADCPose * nearest_pose, size_t * nearest_index,
|
|
|
double * nearest_time, const double max_dist, const double max_yaw)
|
|
|
{
|
|
|
if (traj.empty() || !nearest_pose || !nearest_index || !nearest_time) {
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- const auto autoware_traj = convertToAutowareTrajectory(traj);
|
|
|
- if (autoware_traj.points.empty()) {
|
|
|
- const auto logger = rclcpp::get_logger("mpc_util");
|
|
|
- auto clock = rclcpp::Clock(RCL_ROS_TIME);
|
|
|
- RCLCPP_WARN_THROTTLE(logger, clock, 5000, "[calcNearestPoseInterp] input trajectory is empty");
|
|
|
- return false;
|
|
|
- }
|
|
|
-
|
|
|
- *nearest_index = motion_utils::findFirstNearestIndexWithSoftConstraints(
|
|
|
- autoware_traj.points, self_pose, max_dist, max_yaw);
|
|
|
+ const auto autoware_traj =traj;
|
|
|
+// convertToAutowareTrajectory(traj);
|
|
|
+// if (autoware_traj.points.empty()) {
|
|
|
+// const auto logger = rclcpp::get_logger("mpc_util");
|
|
|
+// auto clock = rclcpp::Clock(RCL_ROS_TIME);
|
|
|
+// RCLCPP_WARN_THROTTLE(logger, clock, 5000, "[calcNearestPoseInterp] input trajectory is empty");
|
|
|
+// return false;
|
|
|
+// }
|
|
|
+
|
|
|
+ *nearest_index = findFirstNearestIndexWithSoftConstraints(
|
|
|
+ autoware_traj, self_pose, max_dist, max_yaw);
|
|
|
const size_t traj_size = traj.size();
|
|
|
|
|
|
if (traj.size() == 1) {
|
|
|
nearest_pose->position.x = traj.x.at(*nearest_index);
|
|
|
nearest_pose->position.y = traj.y.at(*nearest_index);
|
|
|
- nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index));
|
|
|
+ nearest_pose->orientation = ADCcreateQuaternionFromYaw(traj.yaw.at(*nearest_index));
|
|
|
*nearest_time = traj.relative_time.at(*nearest_index);
|
|
|
return true;
|
|
|
}
|
|
|
@@ -378,10 +605,10 @@ bool calcNearestPoseInterp(
|
|
|
return std::make_pair(traj_size - 2, traj_size - 1);
|
|
|
}
|
|
|
|
|
|
- geometry_msgs::msg::Point nearest_traj_point;
|
|
|
+ iv::ADCPoint nearest_traj_point;
|
|
|
nearest_traj_point.x = traj.x.at(*nearest_index);
|
|
|
nearest_traj_point.y = traj.y.at(*nearest_index);
|
|
|
- geometry_msgs::msg::Point next_nearest_traj_point;
|
|
|
+ iv::ADCPoint next_nearest_traj_point;
|
|
|
next_nearest_traj_point.x = traj.x.at(*nearest_index + 1);
|
|
|
next_nearest_traj_point.y = traj.y.at(*nearest_index + 1);
|
|
|
|
|
|
@@ -393,19 +620,19 @@ bool calcNearestPoseInterp(
|
|
|
return std::make_pair(*nearest_index, *nearest_index + 1);
|
|
|
}();
|
|
|
|
|
|
- geometry_msgs::msg::Point next_traj_point;
|
|
|
+ iv::ADCPoint next_traj_point;
|
|
|
next_traj_point.x = traj.x.at(next);
|
|
|
next_traj_point.y = traj.y.at(next);
|
|
|
- geometry_msgs::msg::Point prev_traj_point;
|
|
|
+ iv::ADCPoint prev_traj_point;
|
|
|
prev_traj_point.x = traj.x.at(prev);
|
|
|
prev_traj_point.y = traj.y.at(prev);
|
|
|
const double traj_seg_length =
|
|
|
- tier4_autoware_utils::calcDistance2d(prev_traj_point, next_traj_point);
|
|
|
+ ADCcalcDistance2d(prev_traj_point, next_traj_point);
|
|
|
/* if distance between two points are too close */
|
|
|
if (traj_seg_length < 1.0E-5) {
|
|
|
nearest_pose->position.x = traj.x.at(*nearest_index);
|
|
|
nearest_pose->position.y = traj.y.at(*nearest_index);
|
|
|
- nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index));
|
|
|
+ nearest_pose->orientation = ADCcreateQuaternionFromYaw(traj.yaw.at(*nearest_index));
|
|
|
*nearest_time = traj.relative_time.at(*nearest_index);
|
|
|
return true;
|
|
|
}
|
|
|
@@ -416,27 +643,57 @@ bool calcNearestPoseInterp(
|
|
|
0.0, 1.0);
|
|
|
nearest_pose->position.x = (1 - ratio) * traj.x.at(prev) + ratio * traj.x.at(next);
|
|
|
nearest_pose->position.y = (1 - ratio) * traj.y.at(prev) + ratio * traj.y.at(next);
|
|
|
- const double tmp_yaw_err = normalizeRadian(traj.yaw.at(prev) - traj.yaw.at(next));
|
|
|
- const double nearest_yaw = normalizeRadian(traj.yaw.at(next) + (1 - ratio) * tmp_yaw_err);
|
|
|
- nearest_pose->orientation = createQuaternionFromYaw(nearest_yaw);
|
|
|
+ const double tmp_yaw_err = ADCnormalizeRadian(traj.yaw.at(prev) - traj.yaw.at(next));
|
|
|
+ const double nearest_yaw = ADCnormalizeRadian(traj.yaw.at(next) + (1 - ratio) * tmp_yaw_err);
|
|
|
+ nearest_pose->orientation = ADCcreateQuaternionFromYaw(nearest_yaw);
|
|
|
*nearest_time = (1 - ratio) * traj.relative_time.at(prev) + ratio * traj.relative_time.at(next);
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
-double calcStopDistance(const Trajectory & current_trajectory, const int origin)
|
|
|
+//double calcStopDistance(const Trajectory & current_trajectory, const int origin)
|
|
|
+//{
|
|
|
+// constexpr float zero_velocity = std::numeric_limits<float>::epsilon();
|
|
|
+// const float origin_velocity =
|
|
|
+// current_trajectory.points.at(static_cast<size_t>(origin)).longitudinal_velocity_mps;
|
|
|
+// double stop_dist = 0.0;
|
|
|
+
|
|
|
+// // search forward
|
|
|
+// if (std::fabs(origin_velocity) > zero_velocity) {
|
|
|
+// for (int i = origin + 1; i < static_cast<int>(current_trajectory.points.size()) - 1; ++i) {
|
|
|
+// const auto & p0 = current_trajectory.points.at(i);
|
|
|
+// const auto & p1 = current_trajectory.points.at(i - 1);
|
|
|
+// stop_dist += calcDistance2d(p0, p1);
|
|
|
+// if (std::fabs(p0.longitudinal_velocity_mps) < zero_velocity) {
|
|
|
+// break;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// return stop_dist;
|
|
|
+// }
|
|
|
+
|
|
|
+// // search backward
|
|
|
+// for (int i = origin - 1; 0 < i; --i) {
|
|
|
+// const auto & p0 = current_trajectory.points.at(i);
|
|
|
+// const auto & p1 = current_trajectory.points.at(i + 1);
|
|
|
+// if (std::fabs(p0.longitudinal_velocity_mps) > zero_velocity) {
|
|
|
+// break;
|
|
|
+// }
|
|
|
+// stop_dist -= calcDistance2d(p0, p1);
|
|
|
+// }
|
|
|
+// return stop_dist;
|
|
|
+//}
|
|
|
+
|
|
|
+double calcStopDistance(const MPCTrajectory & current_trajectory, const int origin)
|
|
|
{
|
|
|
constexpr float zero_velocity = std::numeric_limits<float>::epsilon();
|
|
|
- const float origin_velocity =
|
|
|
- current_trajectory.points.at(static_cast<size_t>(origin)).longitudinal_velocity_mps;
|
|
|
+ const float origin_velocity = current_trajectory.vx[origin];
|
|
|
+ // current_trajectory.points.at(static_cast<size_t>(origin)).longitudinal_velocity_mps;
|
|
|
double stop_dist = 0.0;
|
|
|
|
|
|
// search forward
|
|
|
if (std::fabs(origin_velocity) > zero_velocity) {
|
|
|
- for (int i = origin + 1; i < static_cast<int>(current_trajectory.points.size()) - 1; ++i) {
|
|
|
- const auto & p0 = current_trajectory.points.at(i);
|
|
|
- const auto & p1 = current_trajectory.points.at(i - 1);
|
|
|
- stop_dist += calcDistance2d(p0, p1);
|
|
|
- if (std::fabs(p0.longitudinal_velocity_mps) < zero_velocity) {
|
|
|
+ for (int i = origin + 1; i < static_cast<int>(current_trajectory.size()) - 1; ++i) {
|
|
|
+ stop_dist += calcDistance2d(current_trajectory,i-1,i);
|
|
|
+ if (std::fabs(current_trajectory.vx[i]) < zero_velocity) {
|
|
|
break;
|
|
|
}
|
|
|
}
|
|
|
@@ -445,12 +702,10 @@ double calcStopDistance(const Trajectory & current_trajectory, const int origin)
|
|
|
|
|
|
// search backward
|
|
|
for (int i = origin - 1; 0 < i; --i) {
|
|
|
- const auto & p0 = current_trajectory.points.at(i);
|
|
|
- const auto & p1 = current_trajectory.points.at(i + 1);
|
|
|
- if (std::fabs(p0.longitudinal_velocity_mps) > zero_velocity) {
|
|
|
+ if (std::fabs(current_trajectory.vx[i]) > zero_velocity) {
|
|
|
break;
|
|
|
}
|
|
|
- stop_dist -= calcDistance2d(p0, p1);
|
|
|
+ stop_dist -= calcDistance2d(current_trajectory,i,i+1);
|
|
|
}
|
|
|
return stop_dist;
|
|
|
}
|
|
|
@@ -462,8 +717,12 @@ void extendTrajectoryInYawDirection(
|
|
|
traj.yaw.back() = yaw;
|
|
|
|
|
|
// get terminal pose
|
|
|
- const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(traj);
|
|
|
- auto extended_pose = autoware_traj.points.back().pose;
|
|
|
+// const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(traj);
|
|
|
+// auto extended_pose = autoware_traj.points.back().pose;
|
|
|
+
|
|
|
+ double extended_x = traj.back().x;
|
|
|
+ double extended_y = traj.back().y;
|
|
|
+ double extended_z = traj.back().z;
|
|
|
|
|
|
constexpr double extend_dist = 10.0;
|
|
|
constexpr double extend_vel = 10.0;
|
|
|
@@ -471,10 +730,16 @@ void extendTrajectoryInYawDirection(
|
|
|
const double dt = interval / extend_vel;
|
|
|
const size_t num_extended_point = static_cast<size_t>(extend_dist / interval);
|
|
|
for (size_t i = 0; i < num_extended_point; ++i) {
|
|
|
- extended_pose = tier4_autoware_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0);
|
|
|
+ extended_x = extended_x + x_offset*cos(yaw);
|
|
|
+ extended_y = extended_y + x_offset*sin(yaw);
|
|
|
+ extended_z = extended_z;
|
|
|
traj.push_back(
|
|
|
- extended_pose.position.x, extended_pose.position.y, extended_pose.position.z, traj.yaw.back(),
|
|
|
- extend_vel, traj.k.back(), traj.smooth_k.back(), traj.relative_time.back() + dt);
|
|
|
+ extended_x, extended_y, extended_z, traj.yaw.back(),
|
|
|
+ extend_vel, traj.k.back(), traj.smooth_k.back(), traj.relative_time.back() + dt);
|
|
|
+ // extended_pose = tier4_autoware_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0);
|
|
|
+// traj.push_back(
|
|
|
+// extended_pose.position.x, extended_pose.position.y, extended_pose.position.z, traj.yaw.back(),
|
|
|
+// extend_vel, traj.k.back(), traj.smooth_k.back(), traj.relative_time.back() + dt);
|
|
|
}
|
|
|
}
|
|
|
|
