refactor(behavior_tree_plugin): refactor action_node::calculateUpdate…

…dEntityStatusInWorldFrame

common/math/geometry/include/geometry/vector3/rotate.hpp

@@ -0,0 +1,59 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef GEOMETRY__VECTOR3__ROTATE_HPP_
+#define GEOMETRY__VECTOR3__ROTATE_HPP_
+
+#include <geometry/vector3/is_like_vector3.hpp>
+#include <scenario_simulator_exception/exception.hpp>
+
+namespace math
+{
+namespace geometry
+{
+inline Eigen::Vector3d axisToEigenAxis(Axis axis)
+{
+  switch (axis) {
+    case Axis::X:
+      return Eigen::Vector3d::UnitX();
+    case Axis::Y:
+      return Eigen::Vector3d::UnitY();
+    case Axis::Z:
+      return Eigen::Vector3d::UnitZ();
+    default:
+      THROW_SIMULATION_ERROR("Invalid axis specified.");
+  }
+}
+
+// Rotate a vector by a given angle around a specified axis
+template <
+  typename T, std::enable_if_t<std::conjunction_v<IsLikeVector3<T>>, std::nullptr_t> = nullptr>
+auto rotate(T & v, const double angle, const Axis axis)
+{
+  if (!std::isfinite(angle)) {
+    THROW_SIMULATION_ERROR("The provided angle for rotation is not finite.");
+  }
+
+  const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axisToEigenAxis(axis)));
+  const Eigen::Vector3d eigen_vector(v.x, v.y, v.z);
+  const Eigen::Vector3d rotated_vector = rotation * eigen_vector;
+
+  v.x = rotated_vector.x();
+  v.y = rotated_vector.y();
+  v.z = rotated_vector.z();
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__VECTOR3__ROTATE_HPP_

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -21,6 +21,7 @@
 #include <geometry/quaternion/normalize.hpp>
 #include <geometry/quaternion/quaternion_to_euler.hpp>
 #include <geometry/vector3/normalize.hpp>
+#include <geometry/vector3/rotate.hpp>
 #include <memory>
 #include <optional>
 #include <rclcpp/rclcpp.hpp>
@@ -516,27 +517,26 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   using math::geometry::operator+;
   using math::geometry::operator-;
   using math::geometry::operator+=;
+  static bool is_yaw_applied = false;
 
-  const auto getEntityYaw = [&](const geometry_msgs::msg::Quaternion & orientation) {
-    return math::geometry::convertQuaternionToEulerAngle(orientation).z;
-  };
-
-  const auto getLaneletPitch = [&](const lanelet::Id lanelet_id, const double s) {
-    return math::geometry::convertQuaternionToEulerAngle(
-             hdmap_utils->toMapOrientation(lanelet_id, s))
-      .y;
+  // Apply pitch rotation based on the lanelet's pitch angle
+  auto applyPitchRotation = [&](auto & disp, const double lanelet_pose_s) {
+    const auto current_lanelet_Id = canonicalized_entity_status->getLaneletId();
+    const auto lanelet_pitch = math::geometry::convertQuaternionToEulerAngle(
+                                 hdmap_utils->toMapOrientation(current_lanelet_Id, lanelet_pose_s))
+                                 .y;
+    math::geometry::rotate(disp, lanelet_pitch, math::geometry::Axis::Y);
   };
 
-  auto applyRotationToDisplacement = [&](
-                                       geometry_msgs::msg::Vector3 & displacement,
-                                       const double angle, const Eigen::Vector3d & axis) {
-    const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axis));
-    const Eigen::Vector3d rotated_displacement =
-      rotation * Eigen::Vector3d(displacement.x, displacement.y, displacement.z);
-
-    displacement.x = rotated_displacement.x();
-    displacement.y = rotated_displacement.y();
-    displacement.z = rotated_displacement.z();
+  // Apply yaw rotation once to avoid cumulative lateral offset errors
+  auto applyYawRotation = [&](auto & disp) {
+    if (!is_yaw_applied) {
+      const auto yaw = math::geometry::convertQuaternionToEulerAngle(
+                         canonicalized_entity_status->getMapPose().orientation)
+                         .z;
+      math::geometry::rotate(disp, yaw, math::geometry::Axis::Z);
+      is_yaw_applied = true;
+    }
   };
 
   const auto speed_planner =
@@ -549,20 +549,19 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   geometry_msgs::msg::Accel accel_new = std::get<1>(dynamics);
   geometry_msgs::msg::Twist twist_new = std::get<0>(dynamics);
   geometry_msgs::msg::Pose pose_new = canonicalized_entity_status->getMapPose();
-  static bool is_yaw_applied = false;
-
   const auto desired_velocity = geometry_msgs::build<geometry_msgs::msg::Vector3>()
                                   .x(twist_new.linear.x)
                                   .y(twist_new.linear.y)
                                   .z(twist_new.linear.z);
   auto displacement = desired_velocity * step_time;
 
+  // Adjust the entity's position when approaching the end of the current lanelet
   auto adjustPositionAtLaneletBoundary =
     [&](double remaining_lanelet_length, const std::optional<lanelet::Id> & next_lanelet_id) {
       const auto excess_displacement =
         displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length;
 
-      // Update position to the next lanelet if available, otherwise apply full displacement.
+      // If a next lanelet is available, transition to it; otherwise, apply displacement
       pose_new.position = next_lanelet_id
                             ? hdmap_utils->toMapPosition(
                                 next_lanelet_id.value(), math::geometry::norm(excess_displacement))
@@ -577,22 +576,16 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   if (!canonicalized_entity_status->laneMatchingSucceed()) {
     displacement = math::geometry::normalize(pose_new.orientation) * displacement;
     pose_new.position += displacement;
-
   } else {
     const auto current_lanelet_Id = canonicalized_entity_status->getLaneletId();
-    const auto lanelet_pose = canonicalized_entity_status->getLaneletPose().s;
-    applyRotationToDisplacement(
-      displacement, getLaneletPitch(current_lanelet_Id, lanelet_pose), Eigen::Vector3d::UnitY());
-    const double remaining_lanelet_length =
-      hdmap_utils->getLaneletLength(current_lanelet_Id) - lanelet_pose;
+    const auto lanelet_pose_s = canonicalized_entity_status->getLaneletPose().s;
+    const auto remaining_lanelet_length =
+      hdmap_utils->getLaneletLength(current_lanelet_Id) - lanelet_pose_s;
 
-    if (!is_yaw_applied) {
-      const double yaw = getEntityYaw(canonicalized_entity_status->getMapPose().orientation);
-      applyRotationToDisplacement(displacement, yaw, Eigen::Vector3d::UnitZ());
-      is_yaw_applied = true;
-    }
+    applyPitchRotation(displacement, lanelet_pose_s);
+    applyYawRotation(displacement);
 
-    // Adjust position if displacement exceeds the current lanelet length.
+    // Check if the displacement exceeds the remaining lanelet length
     if (math::geometry::norm(displacement) > remaining_lanelet_length) {
       const auto next_lanelet_ids = hdmap_utils->getNextLaneletIds(current_lanelet_Id);
       adjustPositionAtLaneletBoundary(