diff --git a/common/autoware_trajectory/src/pose.cpp b/common/autoware_trajectory/src/pose.cpp index 37dca2f24a..56f6975bc2 100644 --- a/common/autoware_trajectory/src/pose.cpp +++ b/common/autoware_trajectory/src/pose.cpp @@ -117,17 +117,35 @@ void Trajectory::align_orientation_with_trajectory_direction() tf2::Quaternion current_orientation_tf2( current_orientation.x, current_orientation.y, current_orientation.z, current_orientation.w); current_orientation_tf2.normalize(); - const tf2::Vector3 x_axis(1.0, 0.0, 0.0); - const tf2::Vector3 current_x_axis = tf2::quatRotate(current_orientation_tf2, x_axis); + // Calculate the current x-axis of the orientation (local x-axis) + const tf2::Vector3 current_x_axis = + tf2::quatRotate(current_orientation_tf2, tf2::Vector3(1, 0, 0)); + + // Calculate the desired x-axis direction based on the trajectory's azimuth and elevation const tf2::Vector3 desired_x_axis( std::cos(elevation) * std::cos(azimuth), std::cos(elevation) * std::sin(azimuth), std::sin(elevation)); - const tf2::Vector3 rotation_axis = current_x_axis.cross(desired_x_axis).normalized(); + const double dot_product = current_x_axis.dot(desired_x_axis); const double rotation_angle = std::acos(dot_product); - const tf2::Quaternion delta_q(rotation_axis, rotation_angle); + const tf2::Vector3 rotation_axis = [&]() { + // If the rotation angle is nearly 0 or 180 degrees, choose a default rotation axis + if (std::abs(rotation_angle) < 1e-6 || std::abs(rotation_angle - M_PI) < 1e-6) { + // Use the rotated z-axis as the fallback axis + return tf2::quatRotate(current_orientation_tf2, tf2::Vector3(0, 0, 1)); + } + // Otherwise, compute the rotation axis using the cross product of the current and desired + // x-axes + tf2::Vector3 cross = current_x_axis.cross(desired_x_axis); + return cross.normalized(); + }(); + + // Create a quaternion representing the rotation required to align the x-axis + tf2::Quaternion delta_q = tf2::Quaternion(rotation_axis, rotation_angle); + + // Apply the rotation delta to the current orientation and normalize the result const tf2::Quaternion aligned_orientation_tf2 = (delta_q * current_orientation_tf2).normalized(); @@ -142,7 +160,7 @@ void Trajectory::align_orientation_with_trajectory_direction() const bool success = orientation_interpolator_->build(bases_, aligned_orientations); if (!success) { throw std::runtime_error( - "Failed to build orientation interpolator."); // This Exception should not be thrown. + "Failed to build orientation interpolator."); // This exception should not be thrown. } }