Fix/lanelet slope inaccuracies

common/math/geometry/include/geometry/quaternion/norm.hpp

@@ -0,0 +1,33 @@
+// 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__QUATERNION__NORM_HPP_
+#define GEOMETRY__QUATERNION__NORM_HPP_
+
+#include <cmath>
+#include <geometry/quaternion/is_like_quaternion.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <typename T, std::enable_if_t<IsLikeQuaternion<T>::value, std::nullptr_t> = nullptr>
+auto norm(const T & q)
+{
+  return std::hypot(std::hypot(q.w, q.x), std::hypot(q.y, q.z));
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__NORM_HPP_

common/math/geometry/include/geometry/quaternion/normalize.hpp

@@ -0,0 +1,43 @@
+// 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__QUATERNION__NORMALIZE_HPP_
+#define GEOMETRY__QUATERNION__NORMALIZE_HPP_
+
+#include <geometry/quaternion/is_like_quaternion.hpp>
+#include <geometry/quaternion/norm.hpp>
+#include <geometry/quaternion/operator.hpp>
+#include <scenario_simulator_exception/exception.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <typename T, std::enable_if_t<IsLikeQuaternion<T>::value, std::nullptr_t> = nullptr>
+auto normalize(const T & q)
+{
+  const auto n = norm(q);
+  if (std::fabs(n) <= std::numeric_limits<double>::epsilon()) {
+    THROW_SIMULATION_ERROR(
+      "Norm of quaternion (", q.w, ",", q.x, ",", q.y, ",", q.z, ") is ", n,
+      ". The norm of the quaternion you want to normalize should be greater than ",
+      std::numeric_limits<double>::epsilon());
+  }
+  return geometry_msgs::build<geometry_msgs::msg::Quaternion>().x(q.x / n).y(q.y / n).z(q.z / n).w(
+    q.w / n);
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__NORMALIZE_HPP_

common/math/geometry/include/geometry/quaternion/operator.hpp

@@ -16,6 +16,7 @@
 #define GEOMETRY__QUATERNION__OPERATOR_HPP_
 
 #include <geometry/quaternion/is_like_quaternion.hpp>
+#include <geometry/vector3/vector3.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
 
 namespace math
@@ -64,6 +65,34 @@ auto operator*(const T & a, const U & b)
   return v;
 }
 
+template <
+  typename T, typename U,
+  std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>, IsLikeVector3<U>>, std::nullptr_t> =
+    nullptr>
+auto operator*(const T & a, const U & b)
+{
+  T b_quat;
+  b_quat.x = b.x;
+  b_quat.y = b.y;
+  b_quat.z = b.z;
+  b_quat.w = 0.0;
+
+  T a_inv = a;
+  a_inv.x = -a.x;
+  a_inv.y = -a.y;
+  a_inv.z = -a.z;
+  a_inv.w = a.w;
+
+  T result_quat = a * b_quat * a_inv;
+
+  U rotated_vector;
+  rotated_vector.x = result_quat.x;
+  rotated_vector.y = result_quat.y;
+  rotated_vector.z = result_quat.z;
+
+  return rotated_vector;
+}
+
 template <
   typename T, typename U,
   std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>, IsLikeQuaternion<U>>, std::nullptr_t> =

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -18,7 +18,9 @@
 #include <geometry/quaternion/euler_to_quaternion.hpp>
 #include <geometry/quaternion/get_rotation.hpp>
 #include <geometry/quaternion/get_rotation_matrix.hpp>
+#include <geometry/quaternion/normalize.hpp>
 #include <geometry/quaternion/quaternion_to_euler.hpp>
+#include <geometry/vector3/normalize.hpp>
 #include <memory>
 #include <optional>
 #include <rclcpp/rclcpp.hpp>
@@ -511,6 +513,28 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   -> traffic_simulator::EntityStatus
 {
   using math::geometry::operator*;
+  using math::geometry::operator+;
+  using math::geometry::operator-;
+  using math::geometry::operator+=;
+
+  const auto getLaneletPitch = [&](const lanelet::Id lanelet_id, const double s) {
+    const auto lanelet_orientation = hdmap_utils->toMapOrientation(lanelet_id, s);
+    const auto lanelet_rpy = math::geometry::convertQuaternionToEulerAngle(lanelet_orientation);
+
+    return lanelet_rpy.y;
+  };
+
+  auto applyPitchToDisplacement =
+    [&](geometry_msgs::msg::Vector3 & displacement, const double pitch) {
+      const Eigen::Quaterniond pitch_rotation(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()));
+      const Eigen::Vector3d rotated_displacement =
+        pitch_rotation * Eigen::Vector3d(displacement.x, displacement.y, displacement.z);
+
+      displacement.x = rotated_displacement.x();
+      displacement.y = rotated_displacement.y();
+      displacement.z = rotated_displacement.z();
+    };
+
   const auto speed_planner =
     traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner(
       step_time, canonicalized_entity_status->getName());
@@ -520,21 +544,52 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   double linear_jerk_new = std::get<2>(dynamics);
   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;
-  geometry_msgs::msg::Vector3 angular_trans_vec;
-  angular_trans_vec.z = twist_new.angular.z * step_time;
-  geometry_msgs::msg::Quaternion angular_trans_quat =
-    math::geometry::convertEulerAngleToQuaternion(angular_trans_vec);
-  pose_new.orientation = canonicalized_entity_status->getMapPose().orientation * angular_trans_quat;
-  Eigen::Vector3d trans_vec;
-  trans_vec(0) = twist_new.linear.x * step_time;
-  trans_vec(1) = twist_new.linear.y * step_time;
-  trans_vec(2) = 0;
-  Eigen::Matrix3d rotation_mat = math::geometry::getRotationMatrix(pose_new.orientation);
-  trans_vec = rotation_mat * trans_vec;
-  pose_new.position.x = trans_vec(0) + canonicalized_entity_status->getMapPose().position.x;
-  pose_new.position.y = trans_vec(1) + canonicalized_entity_status->getMapPose().position.y;
-  pose_new.position.z = trans_vec(2) + canonicalized_entity_status->getMapPose().position.z;
+  geometry_msgs::msg::Pose pose_new = canonicalized_entity_status->getMapPose();
+
+  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;
+  displacement = math::geometry::normalize(pose_new.orientation) * displacement;
+
+  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.
+      pose_new.position = next_lanelet_id
+                            ? hdmap_utils->toMapPosition(
+                                next_lanelet_id.value(), math::geometry::norm(excess_displacement))
+                            : pose_new.position + displacement;
+
+      // Apply lateral offset if transitioning to the next lanelet
+      if (next_lanelet_id) {
+        pose_new.position.y += canonicalized_entity_status->getLaneletPose().offset;
+      }
+    };
+
+  if (!canonicalized_entity_status->laneMatchingSucceed()) {
+    pose_new.position += displacement;
+  } else {
+    const auto current_lanelet_Id = canonicalized_entity_status->getLaneletId();
+    const auto lanelet_pose = canonicalized_entity_status->getLaneletPose().s;
+    applyPitchToDisplacement(displacement, getLaneletPitch(current_lanelet_Id, lanelet_pose));
+    const double remaining_lanelet_length =
+      hdmap_utils->getLaneletLength(current_lanelet_Id) - lanelet_pose;
+
+    // Adjust position if displacement exceeds the current lanelet length.
+    if (math::geometry::norm(displacement) > remaining_lanelet_length) {
+      const auto next_lanelet_ids = hdmap_utils->getNextLaneletIds(current_lanelet_Id);
+      adjustPositionAtLaneletBoundary(
+        remaining_lanelet_length,
+        next_lanelet_ids.empty() ? std::nullopt : std::make_optional(next_lanelet_ids[0]));
+    } else {
+      pose_new.position += displacement;
+    }
+  }
+
   auto entity_status_updated =
     static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status);
   entity_status_updated.time = current_time + step_time;

simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp

@@ -162,6 +162,16 @@ class HdMapUtils
 
   auto getAltitude(const traffic_simulator_msgs::msg::LaneletPose &) const -> double;
 
+  auto getNextLaneletOnRoute(
+    const lanelet::Id current_lanelet_id, const lanelet::Id destination_lanelet_id) const
+    -> std::optional<lanelet::Id>;
+
+  auto toMapPosition(const lanelet::Id lanelet_id, const double s) const
+    -> geometry_msgs::msg::Point;
+
+  auto toMapOrientation(const lanelet::Id lanelet_id, const double s, const bool fill_pitch = true)
+    const -> geometry_msgs::msg::Quaternion;
+
   auto getLaneChangeTrajectory(
     const geometry_msgs::msg::Pose & from,
     const traffic_simulator::lane_change::Parameter & lane_change_parameter,

simulation/traffic_simulator/src/behavior/follow_trajectory.cpp

@@ -551,8 +551,45 @@ auto makeUpdatedStatus(
        steering.
     */
     auto updated_status = entity_status;
-
-    updated_status.pose.position += desired_velocity * step_time;
+    const auto displacement = desired_velocity * step_time;
+
+    auto adjustPositionAtLaneletBoundary =
+      [&](double remaining_lanelet_length, const std::optional<lanelet::Id> & next_lanelet_id) {
+        const auto excess_displacement =
+          displacement - normalize(desired_velocity) * remaining_lanelet_length;
+
+        // Update position to the next lanelet if available, otherwise apply full displacement.
+        updated_status.pose.position =
+          next_lanelet_id
+            ? hdmap_utils->toMapPosition(next_lanelet_id.value(), norm(excess_displacement))
+            : updated_status.pose.position + displacement;
+
+        // Apply lateral offset if transitioning to the next lanelet
+        if (next_lanelet_id) {
+          updated_status.pose.position.y += updated_status.lanelet_pose.offset;
+        }
+      };
+
+    if (!updated_status.lanelet_pose_valid) {
+      updated_status.pose.position += displacement;
+    } else {
+      const double remaining_lanelet_length =
+        hdmap_utils->getLaneletLength(updated_status.lanelet_pose.lanelet_id) -
+        updated_status.lanelet_pose.s;
+
+      // Adjust position if displacement exceeds the current lanelet length.
+      if (norm(displacement) > remaining_lanelet_length) {
+        const auto target_lanelet_pose = hdmap_utils->toLaneletPose(
+          target_position, updated_status.bounding_box, false, matching_distance);
+        adjustPositionAtLaneletBoundary(
+          remaining_lanelet_length, target_lanelet_pose ? hdmap_utils->getNextLaneletOnRoute(
+                                                            updated_status.lanelet_pose.lanelet_id,
+                                                            target_lanelet_pose->lanelet_id)
+                                                        : std::nullopt);
+      } else {
+        updated_status.pose.position += displacement;
+      }
+    }
 
     updated_status.pose.orientation = [&]() {
       if (desired_velocity.y == 0 && desired_velocity.x == 0 && desired_velocity.z == 0) {

simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp

@@ -1098,6 +1098,33 @@ auto HdMapUtils::getNextLaneletIds(
   return sortAndUnique(ids);
 }
 
+auto HdMapUtils::getNextLaneletOnRoute(
+  const lanelet::Id current_lanelet_id, const lanelet::Id destination_lanelet_id) const
+  -> std::optional<lanelet::Id>
+{
+  const auto route = getRoute(current_lanelet_id, destination_lanelet_id);
+
+  if (const auto it = std::find(route.begin(), route.end(), current_lanelet_id);
+      it != route.end() && std::next(it) != route.end()) {
+    return *std::next(it);
+  }
+
+  return std::nullopt;
+}
+
+auto HdMapUtils::toMapPosition(const lanelet::Id lanelet_id, const double s) const
+  -> geometry_msgs::msg::Point
+{
+  return getCenterPointsSpline(lanelet_id)->getPoint(s);
+}
+
+auto HdMapUtils::toMapOrientation(
+  const lanelet::Id lanelet_id, const double s, const bool fill_pitch) const
+  -> geometry_msgs::msg::Quaternion
+{
+  return getCenterPointsSpline(lanelet_id)->getPose(s, fill_pitch).orientation;
+}
+
 auto HdMapUtils::getTrafficLightIds() const -> lanelet::Ids
 {
   using namespace lanelet::utils::query;