diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 78b6daa9034fe..912502ed9e61e 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -89,6 +89,45 @@ void validateNonSharpAngle( } } +template +bool isDrivingForward(const T points) +{ + if (points.size() < 2) { + return true; + } + + // 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; + } + + return false; +} + +template +bool isDrivingForwardWithTwist(const T points_with_twist) +{ + if (points_with_twist.empty()) { + return true; + } + if (points_with_twist.size() == 1) { + if (0.0 <= tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { + return true; + } + return false; + } + + return isDrivingForward(points_with_twist); +} + template boost::optional searchZeroVelocityIndex( const T & points_with_twist, const size_t src_idx, const size_t dst_idx) @@ -805,10 +844,13 @@ inline boost::optional insertTargetPoint( const auto overlap_with_back = tier4_autoware_utils::calcDistance2d(p_target, p_back) < overlap_threshold; + const bool is_driving_forward = isDrivingForward(points); + geometry_msgs::msg::Pose target_pose; { - const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_back); - const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_back); + const auto p_base = is_driving_forward ? p_back : p_front; + const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_base); + const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_base); target_pose.position = p_target; target_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); @@ -817,17 +859,22 @@ inline boost::optional insertTargetPoint( auto p_insert = points.at(seg_idx); tier4_autoware_utils::setPose(target_pose, p_insert); - geometry_msgs::msg::Pose front_pose; + geometry_msgs::msg::Pose base_pose; { - const auto pitch = tier4_autoware_utils::calcElevationAngle(p_front, p_target); - const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_front, p_target); + const auto p_base = is_driving_forward ? p_front : p_back; + const auto pitch = tier4_autoware_utils::calcElevationAngle(p_base, p_target); + const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_base, p_target); - front_pose.position = tier4_autoware_utils::getPoint(points.at(seg_idx)); - front_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + base_pose.position = tier4_autoware_utils::getPoint(p_base); + base_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); } if (!overlap_with_front && !overlap_with_back) { - tier4_autoware_utils::setPose(front_pose, points.at(seg_idx)); + if (is_driving_forward) { + tier4_autoware_utils::setPose(base_pose, points.at(seg_idx)); + } else { + tier4_autoware_utils::setPose(base_pose, points.at(seg_idx + 1)); + } points.insert(points.begin() + seg_idx + 1, p_insert); return seg_idx + 1; } @@ -1012,36 +1059,6 @@ inline boost::optional insertStopPoint( return stop_idx; } - -template -inline bool isDrivingForward(const T points_with_twist) -{ - // if points size is smaller than 2 - if (points_with_twist.empty()) { - return true; - } - if (points_with_twist.size() == 1) { - if (0.0 <= tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { - return true; - } - return false; - } - - // check the first point direction - const auto & first_point_pose = tier4_autoware_utils::getPose(points_with_twist.at(0)); - const auto & second_point_pose = tier4_autoware_utils::getPose(points_with_twist.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)) < - M_PI_2) { - return true; - } - - return false; -} } // namespace motion_utils #endif // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 996917d97bfdb..8e3809f739936 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -1691,6 +1691,64 @@ TEST(trajectory, insertTargetPoint) } } +TEST(trajectory, insertTargetPoint_Reverse) +{ + using motion_utils::calcArcLength; + using motion_utils::findNearestSegmentIndex; + using motion_utils::insertTargetPoint; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::createQuaternionFromYaw; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::getPose; + + constexpr double overlap_threshold = 1e-4; + auto traj = generateTestTrajectory(10, 1.0); + for (size_t i = 0; i < traj.points.size(); ++i) { + traj.points.at(i).pose.orientation = createQuaternionFromYaw(tier4_autoware_utils::pi); + } + const auto total_length = calcArcLength(traj.points); + + for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = createPoint(x_start + 1.1e-4, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = + insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE( + calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(180)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(180)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } +} + TEST(trajectory, insertTargetPoint_OverlapThreshold) { using motion_utils::calcArcLength; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 4cb74e52ddae8..658b25a156c37 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -493,7 +493,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // Get Target Obstacles DebugData debug_data; - const bool is_driving_forward = motion_utils::isDrivingForward(msg->points); + const bool is_driving_forward = motion_utils::isDrivingForwardWithTwist(msg->points); const auto target_obstacles = getTargetObstacles( *msg, current_pose_ptr->pose, current_twist_ptr_->twist.linear.x, is_driving_forward, debug_data);