diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 912502ed9e61e..9e58e43a94f13 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -97,19 +97,10 @@ bool isDrivingForward(const T points) } // check the first point direction - const auto & first_point_pose = tier4_autoware_utils::getPose(points.at(0)); - const auto & second_point_pose = tier4_autoware_utils::getPose(points.at(1)); - - const double first_point_yaw = tf2::getYaw(first_point_pose.orientation); - const double driving_direction_yaw = - tier4_autoware_utils::calcAzimuthAngle(first_point_pose.position, second_point_pose.position); - if ( - std::abs(tier4_autoware_utils::normalizeRadian(first_point_yaw - driving_direction_yaw)) < - tier4_autoware_utils::pi / 2.0) { - return true; - } + const auto & first_pose = tier4_autoware_utils::getPose(points.at(0)); + const auto & second_pose = tier4_autoware_utils::getPose(points.at(1)); - return false; + return tier4_autoware_utils::isDrivingForward(first_pose, second_pose); } template diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 9794a8ec8706e..bd4c0f861e063 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -21,6 +21,8 @@ #define EIGEN_MPL2_ONLY #include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "tier4_autoware_utils/math/constants.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" #include #include @@ -32,6 +34,8 @@ #include #include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else @@ -581,6 +585,19 @@ inline double calcCurvature( return 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / denominator; } +template +bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose) +{ + // check the first point direction + const double src_yaw = tf2::getYaw(getPose(src_pose).orientation); + const double pose_direction_yaw = calcAzimuthAngle(getPoint(src_pose), getPoint(dst_pose)); + if (std::fabs(normalizeRadian(src_yaw - pose_direction_yaw)) < pi / 2.0) { + return true; + } + + return false; +} + /** * @brief Calculate offset pose. The offset values are defined in the local coordinate of the input * pose. @@ -638,7 +655,7 @@ geometry_msgs::msg::Point calcInterpolatedPoint( /** * @brief Calculate a pose by linear interpolation. - * Note that if ratio>=1.0 or dist(src_pose, dst_pose)<=0.01 + * Note that if dist(src_pose, dst_pose)<=0.01 * the orientation of the output pose is same as the orientation * of the dst_pose * @param src source point @@ -653,20 +670,25 @@ geometry_msgs::msg::Pose calcInterpolatedPose( const bool set_orientation_from_position_direction = true) { const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + geometry_msgs::msg::Pose output_pose; output_pose.position = calcInterpolatedPoint(getPoint(src_pose), getPoint(dst_pose), clamped_ratio); if (set_orientation_from_position_direction) { const double input_poses_dist = calcDistance2d(getPoint(src_pose), getPoint(dst_pose)); + const bool is_driving_forward = isDrivingForward(src_pose, dst_pose); // Get orientation from interpolated point and src_pose - if (clamped_ratio < 1.0 && input_poses_dist > 1e-3) { - const double pitch = calcElevationAngle(getPoint(output_pose), getPoint(dst_pose)); - const double yaw = calcAzimuthAngle(output_pose.position, getPoint(dst_pose)); - output_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw); - } else { + if ((is_driving_forward && clamped_ratio > 1.0 - (1e-6)) || input_poses_dist < 1e-3) { output_pose.orientation = getPose(dst_pose).orientation; + } else if (!is_driving_forward && clamped_ratio < 1e-6) { + output_pose.orientation = getPose(src_pose).orientation; + } else { + const auto & base_pose = is_driving_forward ? dst_pose : src_pose; + const double pitch = calcElevationAngle(getPoint(output_pose), getPoint(base_pose)); + const double yaw = calcAzimuthAngle(getPoint(output_pose), getPoint(base_pose)); + output_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw); } } else { // Get orientation by spherical linear interpolation diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp index 400cbeb93542e..901232796b5ce 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp @@ -1083,6 +1083,68 @@ TEST(geometry, calcOffsetPose) } } +TEST(geometry, isDrivingForward) +{ + using tier4_autoware_utils::calcInterpolatedPoint; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::createQuaternion; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::isDrivingForward; + + const double epsilon = 1e-3; + + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(3.0, 0.0, 0.0); + dst_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + + EXPECT_TRUE(isDrivingForward(src_pose, dst_pose)); + } + + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(180)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(3.0, 0.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(180)); + + EXPECT_FALSE(isDrivingForward(src_pose, dst_pose)); + } + + // Boundary Condition + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(3.0, 0.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90)); + + EXPECT_TRUE(isDrivingForward(src_pose, dst_pose)); + } + + // Boundary Condition + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90 + epsilon)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(3.0, 0.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90 + epsilon)); + + EXPECT_FALSE(isDrivingForward(src_pose, dst_pose)); + } +} + TEST(geometry, calcInterpolatedPoint) { using tier4_autoware_utils::calcInterpolatedPoint; @@ -1226,7 +1288,7 @@ TEST(geometry, calcInterpolatedPose) dst_pose.position = createPoint(1.0, 1.0, 0.0); dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); - for (double ratio = 0.0; ratio < 1.0; ratio += 0.1) { + for (double ratio = 0.0; ratio < 1.0 - epsilon; ratio += 0.1) { const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio); const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(45)); @@ -1298,6 +1360,30 @@ TEST(geometry, calcInterpolatedPose) EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); } } + + // Driving Backward + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(180)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(5.0, 0.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(180)); + + for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio); + + const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(180)); + EXPECT_DOUBLE_EQ(p_out.position.x, 5.0 * ratio); + EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); + EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); + EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); + EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); + } + } } TEST(geometry, calcInterpolatedPose_with_Spherical_Interpolation)