fix(autoware_trajectory): fix a bug of align_orientation_with_traject…

…ory_direction (#234)

* fix bug of align_orientation_with_trajectory_direction

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

* fixed in a better way

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

* reflect comments

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

* revert unnecessary changes

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

---------

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

common/autoware_trajectory/src/pose.cpp

@@ -117,17 +117,35 @@ void Trajectory<PointType>::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<PointType>::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.
   }
 }