Skip to content

Commit

Permalink
revert unnecessary changes
Browse files Browse the repository at this point in the history
Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
  • Loading branch information
yhisaki committed Mar 4, 2025
1 parent 38884c7 commit cac474e
Showing 1 changed file with 0 additions and 3 deletions.
3 changes: 0 additions & 3 deletions common/autoware_trajectory/src/pose.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,6 @@ void Trajectory<PointType>::align_orientation_with_trajectory_direction()
const double elevation = this->elevation(s);
const geometry_msgs::msg::Quaternion current_orientation =
orientation_interpolator_->compute(s);

tf2::Quaternion current_orientation_tf2(
current_orientation.x, current_orientation.y, current_orientation.z, current_orientation.w);
current_orientation_tf2.normalize();
Expand All @@ -128,7 +127,6 @@ void Trajectory<PointType>::align_orientation_with_trajectory_direction()
std::cos(elevation) * std::cos(azimuth), std::cos(elevation) * std::sin(azimuth),
std::sin(elevation));

// Compute the dot product and the angle between the current and desired x-axes
const double dot_product = current_x_axis.dot(desired_x_axis);
const double rotation_angle = std::acos(dot_product);

Expand All @@ -151,7 +149,6 @@ void Trajectory<PointType>::align_orientation_with_trajectory_direction()
const tf2::Quaternion aligned_orientation_tf2 =
(delta_q * current_orientation_tf2).normalized();

// Convert the tf2 quaternion back to geometry_msgs quaternion format
geometry_msgs::msg::Quaternion aligned_orientation;
aligned_orientation.x = aligned_orientation_tf2.x();
aligned_orientation.y = aligned_orientation_tf2.y();
Expand Down

0 comments on commit cac474e

Please sign in to comment.