From 8dfa732162df0a778107c06cde442f888d25d553 Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Wed, 18 Dec 2024 00:31:10 +0100
Subject: [PATCH 01/38] fix(traffic_simulator) Fix lanelet slope inaccuracies -
 follow trajectory action

---
 .../hdmap_utils/hdmap_utils.hpp               | 10 +++++
 .../src/behavior/follow_trajectory.cpp        | 41 ++++++++++++++++++-
 .../src/hdmap_utils/hdmap_utils.cpp           | 26 ++++++++++++
 3 files changed, 75 insertions(+), 2 deletions(-)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
index 9be623a2cab..82cb9eb8d71 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
+++ b/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
+    -> geometry_msgs::msg::Quaternion;
+
   auto getLaneChangeTrajectory(
     const geometry_msgs::msg::Pose & from,
     const traffic_simulator::lane_change::Parameter & lane_change_parameter,
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index da2ced4710c..d45b02688d7 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/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) {
diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
index eab04aaf2f2..562a0d67d3a 100644
--- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
+++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
@@ -1098,6 +1098,32 @@ 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
+  -> geometry_msgs::msg::Quaternion
+{
+  return getCenterPointsSpline(lanelet_id)->getPose(s, true).orientation;
+}
+
 auto HdMapUtils::getTrafficLightIds() const -> lanelet::Ids
 {
   using namespace lanelet::utils::query;

From 0aae03561deadddb28f716c4c9e343caaa9cc8d8 Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Wed, 18 Dec 2024 09:18:42 +0100
Subject: [PATCH 02/38] fix(behavior_tree_plugin)  Fix lanelet slope
 inaccuracies - walk straight action

---
 .../behavior_tree_plugin/src/action_node.cpp  | 83 +++++++++++++++----
 .../hdmap_utils/hdmap_utils.hpp               |  4 +-
 .../src/hdmap_utils/hdmap_utils.cpp           |  5 +-
 3 files changed, 73 insertions(+), 19 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 86db4b18d41..3c69df7d004 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -19,6 +19,7 @@
 #include <geometry/quaternion/get_rotation.hpp>
 #include <geometry/quaternion/get_rotation_matrix.hpp>
 #include <geometry/quaternion/quaternion_to_euler.hpp>
+#include <geometry/vector3/normalize.hpp>
 #include <memory>
 #include <optional>
 #include <rclcpp/rclcpp.hpp>
@@ -511,6 +512,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 +543,51 @@ 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 current_lanelet_Id = canonicalized_entity_status->getLaneletId();
+
+  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;
+
+  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 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;
diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
index 82cb9eb8d71..9b804fd8290 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
@@ -169,8 +169,8 @@ class HdMapUtils
   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
-    -> geometry_msgs::msg::Quaternion;
+  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,
diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
index 562a0d67d3a..de6e9c92f68 100644
--- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
+++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
@@ -1118,10 +1118,11 @@ auto HdMapUtils::toMapPosition(const lanelet::Id lanelet_id, const double s) con
   return getCenterPointsSpline(lanelet_id)->getPoint(s);
 }
 
-auto HdMapUtils::toMapOrientation(const lanelet::Id lanelet_id, const double s) const
+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, true).orientation;
+  return getCenterPointsSpline(lanelet_id)->getPose(s, fill_pitch).orientation;
 }
 
 auto HdMapUtils::getTrafficLightIds() const -> lanelet::Ids

From a2131850c734639896a211b668c2433a2c24a0fa Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Wed, 18 Dec 2024 13:05:35 +0100
Subject: [PATCH 03/38] fix(behavior_tree_plugin) Fix isssue with pedestrian
 turning

---
 .../include/geometry/quaternion/norm.hpp      | 33 ++++++++++++++
 .../include/geometry/quaternion/normalize.hpp | 43 ++++++++++++++++++
 .../include/geometry/quaternion/operator.hpp  | 29 ++++++++++++
 .../behavior_tree_plugin/src/action_node.cpp  | 45 ++++++++++++-------
 4 files changed, 135 insertions(+), 15 deletions(-)
 create mode 100644 common/math/geometry/include/geometry/quaternion/norm.hpp
 create mode 100644 common/math/geometry/include/geometry/quaternion/normalize.hpp

diff --git a/common/math/geometry/include/geometry/quaternion/norm.hpp b/common/math/geometry/include/geometry/quaternion/norm.hpp
new file mode 100644
index 00000000000..2ef8102b724
--- /dev/null
+++ b/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_
diff --git a/common/math/geometry/include/geometry/quaternion/normalize.hpp b/common/math/geometry/include/geometry/quaternion/normalize.hpp
new file mode 100644
index 00000000000..9b819b7bf3f
--- /dev/null
+++ b/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_
diff --git a/common/math/geometry/include/geometry/quaternion/operator.hpp b/common/math/geometry/include/geometry/quaternion/operator.hpp
index 624dd686852..ca124ed5e8d 100644
--- a/common/math/geometry/include/geometry/quaternion/operator.hpp
+++ b/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> =
diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 3c69df7d004..a6019cf314b 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -18,6 +18,7 @@
 #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>
@@ -516,23 +517,27 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   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);
+  const auto getEntityYaw = [&](const geometry_msgs::msg::Quaternion & orientation) {
+    return math::geometry::convertQuaternionToEulerAngle(orientation).z;
+  };
 
-    return lanelet_rpy.y;
+  const auto getLaneletPitch = [&](const lanelet::Id lanelet_id, const double s) {
+    return math::geometry::convertQuaternionToEulerAngle(
+             hdmap_utils->toMapOrientation(lanelet_id, s))
+      .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);
+  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();
-    };
+    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(
@@ -544,7 +549,7 @@ 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();
-  const auto current_lanelet_Id = canonicalized_entity_status->getLaneletId();
+  static bool is_yaw_applied = false;
 
   const auto desired_velocity = geometry_msgs::build<geometry_msgs::msg::Vector3>()
                                   .x(twist_new.linear.x)
@@ -570,13 +575,23 @@ 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;
-    applyPitchToDisplacement(displacement, getLaneletPitch(current_lanelet_Id, lanelet_pose));
+    applyRotationToDisplacement(
+      displacement, getLaneletPitch(current_lanelet_Id, lanelet_pose), Eigen::Vector3d::UnitY());
     const double remaining_lanelet_length =
       hdmap_utils->getLaneletLength(current_lanelet_Id) - lanelet_pose;
 
+    if (!is_yaw_applied) {
+      const double yaw = getEntityYaw(canonicalized_entity_status->getMapPose().orientation);
+      applyRotationToDisplacement(displacement, yaw, Eigen::Vector3d::UnitZ());
+      is_yaw_applied = true;
+    }
+
     // 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);

From 1fbb1c198fc1662fb158740b801eed21370c8f3b Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Wed, 18 Dec 2024 15:09:25 +0100
Subject: [PATCH 04/38] refactor(behavior_tree_plugin): refactor
 action_node::calculateUpdatedEntityStatusInWorldFrame

---
 .../include/geometry/vector3/rotate.hpp       | 59 +++++++++++++++++++
 .../behavior_tree_plugin/src/action_node.cpp  | 59 ++++++++-----------
 2 files changed, 85 insertions(+), 33 deletions(-)
 create mode 100644 common/math/geometry/include/geometry/vector3/rotate.hpp

diff --git a/common/math/geometry/include/geometry/vector3/rotate.hpp b/common/math/geometry/include/geometry/vector3/rotate.hpp
new file mode 100644
index 00000000000..33648f01f83
--- /dev/null
+++ b/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_
diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index a6019cf314b..7048131138e 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/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(

From 26f8a6547e0c4573a9ca4738e5486335a2c9e8b4 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 01:00:16 +0100
Subject: [PATCH 05/38] ref(geometry): add axis header, improve

---
 .../geometry/include/geometry/axis/axis.hpp   | 41 ++++++++
 .../include/geometry/polygon/polygon.hpp      |  5 -
 .../include/geometry/quaternion/normalize.hpp | 13 ++-
 .../include/geometry/quaternion/operator.hpp  | 30 +++---
 .../include/geometry/vector3/rotate.hpp       | 31 ++----
 common/math/geometry/src/axis/axis.cpp        | 94 +++++++++++++++++++
 common/math/geometry/src/polygon/polygon.cpp  | 67 -------------
 .../test/src/polygon/test_polygon.cpp         |  2 +
 .../primitives/primitive.hpp                  |  1 +
 9 files changed, 170 insertions(+), 114 deletions(-)
 create mode 100644 common/math/geometry/include/geometry/axis/axis.hpp
 create mode 100644 common/math/geometry/src/axis/axis.cpp

diff --git a/common/math/geometry/include/geometry/axis/axis.hpp b/common/math/geometry/include/geometry/axis/axis.hpp
new file mode 100644
index 00000000000..e0da906d9f5
--- /dev/null
+++ b/common/math/geometry/include/geometry/axis/axis.hpp
@@ -0,0 +1,41 @@
+
+// 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__AXIS__AXIS_HPP_
+#define GEOMETRY__AXIS__AXIS_HPP_
+
+#include <Eigen/Dense>
+#include <geometry_msgs/msg/point.hpp>
+
+namespace math
+{
+namespace geometry
+{
+enum class Axis { X = 0, Y = 1, Z = 2 };
+
+auto axisToEigenAxis(Axis axis) -> Eigen::Vector3d;
+
+auto getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
+  -> double;
+
+auto getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
+  -> double;
+
+auto filterByAxis(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
+  -> std::vector<double>;
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__AXIS__AXIS_HPP_
diff --git a/common/math/geometry/include/geometry/polygon/polygon.hpp b/common/math/geometry/include/geometry/polygon/polygon.hpp
index 02f0374368c..231dae67ac6 100644
--- a/common/math/geometry/include/geometry/polygon/polygon.hpp
+++ b/common/math/geometry/include/geometry/polygon/polygon.hpp
@@ -21,11 +21,6 @@ namespace math
 {
 namespace geometry
 {
-enum class Axis { X = 0, Y = 1, Z = 2 };
-double getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis);
-double getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis);
-std::vector<double> filterByAxis(
-  const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis);
 std::vector<geometry_msgs::msg::Point> get2DConvexHull(
   const std::vector<geometry_msgs::msg::Point> & points);
 }  // namespace geometry
diff --git a/common/math/geometry/include/geometry/quaternion/normalize.hpp b/common/math/geometry/include/geometry/quaternion/normalize.hpp
index 9b819b7bf3f..8e83a7f364e 100644
--- a/common/math/geometry/include/geometry/quaternion/normalize.hpp
+++ b/common/math/geometry/include/geometry/quaternion/normalize.hpp
@@ -27,15 +27,18 @@ 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()) {
+  if (const auto n = norm(q); std::fabs(n) <= std::numeric_limits<double>::epsilon()) {
     THROW_SIMULATION_ERROR(
-      "Norm of quaternion (", q.w, ",", q.x, ",", q.y, ",", q.z, ") is ", n,
+      "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());
+  } else {
+    return geometry_msgs::build<geometry_msgs::msg::Quaternion>()
+      .x(q.x / n)
+      .y(q.y / n)
+      .z(q.z / n)
+      .w(q.w / n);
   }
-  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
diff --git a/common/math/geometry/include/geometry/quaternion/operator.hpp b/common/math/geometry/include/geometry/quaternion/operator.hpp
index ca124ed5e8d..4839a282333 100644
--- a/common/math/geometry/include/geometry/quaternion/operator.hpp
+++ b/common/math/geometry/include/geometry/quaternion/operator.hpp
@@ -69,26 +69,26 @@ 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)
+auto operator*(const T & rotation, const U & vector)
 {
-  T b_quat;
-  b_quat.x = b.x;
-  b_quat.y = b.y;
-  b_quat.z = b.z;
-  b_quat.w = 0.0;
+  T vector_as_quaternion;
+  vector_as_quaternion.x = vector.x;
+  vector_as_quaternion.y = vector.y;
+  vector_as_quaternion.z = vector.z;
+  vector_as_quaternion.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 inverse_rotation = rotation;
+  inverse_rotation.x = -rotation.x;
+  inverse_rotation.y = -rotation.y;
+  inverse_rotation.z = -rotation.z;
+  inverse_rotation.w = rotation.w;
 
-  T result_quat = a * b_quat * a_inv;
+  T rotated_quaternion = rotation * vector_as_quaternion * inverse_rotation;
 
   U rotated_vector;
-  rotated_vector.x = result_quat.x;
-  rotated_vector.y = result_quat.y;
-  rotated_vector.z = result_quat.z;
+  rotated_vector.x = vector_as_quaternion.x;
+  rotated_vector.y = vector_as_quaternion.y;
+  rotated_vector.z = vector_as_quaternion.z;
 
   return rotated_vector;
 }
diff --git a/common/math/geometry/include/geometry/vector3/rotate.hpp b/common/math/geometry/include/geometry/vector3/rotate.hpp
index 33648f01f83..91c5b592bae 100644
--- a/common/math/geometry/include/geometry/vector3/rotate.hpp
+++ b/common/math/geometry/include/geometry/vector3/rotate.hpp
@@ -15,6 +15,7 @@
 #ifndef GEOMETRY__VECTOR3__ROTATE_HPP_
 #define GEOMETRY__VECTOR3__ROTATE_HPP_
 
+#include <geometry/axis/axis.hpp>
 #include <geometry/vector3/is_like_vector3.hpp>
 #include <scenario_simulator_exception/exception.hpp>
 
@@ -22,20 +23,6 @@ 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>
@@ -43,15 +30,15 @@ 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;
+  } else {
+    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();
+    v.x = rotated_vector.x();
+    v.y = rotated_vector.y();
+    v.z = rotated_vector.z();
+  }
 }
 }  // namespace geometry
 }  // namespace math
diff --git a/common/math/geometry/src/axis/axis.cpp b/common/math/geometry/src/axis/axis.cpp
new file mode 100644
index 00000000000..422ddbbc36c
--- /dev/null
+++ b/common/math/geometry/src/axis/axis.cpp
@@ -0,0 +1,94 @@
+// 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.
+
+#include <boost/geometry.hpp>
+// #include <boost/geometry/geometries/point_xy.hpp>
+// #include <boost/geometry/geometries/polygon.hpp>
+// #include <geometry/polygon/polygon.hpp>
+// #include <rclcpp/rclcpp.hpp>
+#include <geometry/axis/axis.hpp>
+#include <scenario_simulator_exception/exception.hpp>
+
+namespace math
+{
+namespace geometry
+{
+auto axisToEigenAxis(Axis axis) -> Eigen::Vector3d
+{
+  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.");
+  }
+}
+
+auto getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) -> double
+{
+  if (const auto values = filterByAxis(points, axis); values.size() == 1) {
+    return values.front();
+  } else {
+    return *std::max_element(values.begin(), values.end());
+  }
+}
+
+auto getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) -> double
+{
+  if (const auto values = filterByAxis(points, axis); values.size() == 1) {
+    return values.front();
+  } else {
+    return *std::min_element(values.begin(), values.end());
+  }
+}
+
+auto filterByAxis(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
+  -> std::vector<double>
+{
+  if (points.empty()) {
+    THROW_SIMULATION_ERROR(
+      "Invalid point list is specified, while trying to filter ",
+      axis == Axis::X   ? "X"
+      : axis == Axis::Y ? "Y"
+                        : "Z",
+      " axis. ",
+      "The point list in filterByAxis() should have at least one point to filter. "
+      "This message is not originally intended to be displayed, if you see it, please "
+      "contact the developer of traffic_simulator.");
+  } else {
+    const auto axisExtractor =
+      [](const Axis axis) -> std::function<double(const geometry_msgs::msg::Point &)> {
+      switch (axis) {
+        case Axis::X:
+          return [](const geometry_msgs::msg::Point & p) { return p.x; };
+        case Axis::Y:
+          return [](const geometry_msgs::msg::Point & p) { return p.y; };
+        case Axis::Z:
+          return [](const geometry_msgs::msg::Point & p) { return p.z; };
+        default:
+          throw std::invalid_argument("Invalid axis");
+      }
+    };
+
+    std::vector<double> single_axis;
+    std::transform(
+      points.begin(), points.end(), std::back_inserter(single_axis), axisExtractor(axis));
+    return single_axis;
+  }
+}
+}  // namespace geometry
+}  // namespace math
diff --git a/common/math/geometry/src/polygon/polygon.cpp b/common/math/geometry/src/polygon/polygon.cpp
index 7a7148b40d8..06886e6299a 100644
--- a/common/math/geometry/src/polygon/polygon.cpp
+++ b/common/math/geometry/src/polygon/polygon.cpp
@@ -47,72 +47,5 @@ std::vector<geometry_msgs::msg::Point> get2DConvexHull(
   }
   return polygon;
 }
-
-double getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
-{
-  if (points.empty()) {
-    THROW_SIMULATION_ERROR(
-      "Invalid point list is specified, while getting max value on ",
-      axis == Axis::X   ? "X"
-      : axis == Axis::Y ? "Y"
-                        : "Z",
-      " axis. ",
-      "The point list in getMaxValue should have at least one point to get the max value from. "
-      "This message is not originally intended to be displayed, if you see it, please "
-      "contact the developer of traffic_simulator.");
-  }
-  const auto values = filterByAxis(points, axis);
-  if (values.size() == 1) {
-    return values.front();
-  }
-  return *std::max_element(values.begin(), values.end());
-}
-
-double getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
-{
-  if (points.empty()) {
-    THROW_SIMULATION_ERROR(
-      "Invalid point list is specified, while getting min value on ",
-      axis == Axis::X   ? "X"
-      : axis == Axis::Y ? "Y"
-                        : "Z",
-      " axis. ",
-      "The point list in getMinValue should have at least one point to get the min value from. "
-      "This message is not originally intended to be displayed, if you see it, please "
-      "contact the developer of traffic_simulator.");
-  }
-  const auto values = filterByAxis(points, axis);
-  if (values.size() == 1) {
-    return values.front();
-  }
-  return *std::min_element(values.begin(), values.end());
-}
-
-std::vector<double> filterByAxis(
-  const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
-{
-  std::vector<double> ret;
-  switch (axis) {
-    case Axis::X: {
-      std::transform(
-        points.begin(), points.end(), std::back_inserter(ret),
-        [](const geometry_msgs::msg::Point & point) { return point.x; });
-      break;
-    }
-    case Axis::Y: {
-      std::transform(
-        points.begin(), points.end(), std::back_inserter(ret),
-        [](const geometry_msgs::msg::Point & point) { return point.y; });
-      break;
-    }
-    case Axis::Z: {
-      std::transform(
-        points.begin(), points.end(), std::back_inserter(ret),
-        [](const geometry_msgs::msg::Point & point) { return point.z; });
-      break;
-    }
-  }
-  return ret;
-}
 }  // namespace geometry
 }  // namespace math
diff --git a/common/math/geometry/test/src/polygon/test_polygon.cpp b/common/math/geometry/test/src/polygon/test_polygon.cpp
index e7278fa6566..9b4596e9913 100644
--- a/common/math/geometry/test/src/polygon/test_polygon.cpp
+++ b/common/math/geometry/test/src/polygon/test_polygon.cpp
@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#include <geometry/axis/axis.hpp>
 #include <geometry/polygon/polygon.hpp>
 #include <scenario_simulator_exception/exception.hpp>
 
@@ -41,6 +42,7 @@ TEST(Polygon, filterByAxis)
   EXPECT_DOUBLE_EQ(values_z[2], -3.0);
 }
 
+/// @todo refactor test to throw exception
 TEST(Polygon, filterByAxisEmptyVector)
 {
   std::vector<geometry_msgs::msg::Point> points;
diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp
index c514d40247e..b9c6b9d2fb4 100644
--- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp
+++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp
@@ -18,6 +18,7 @@
 #include <embree4/rtcore.h>
 
 #include <algorithm>
+#include <geometry/axis/axis.hpp>
 #include <geometry/polygon/polygon.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <optional>

From 6a8f3ce1b66dce410483ca1656028f5822d45661 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 01:01:29 +0100
Subject: [PATCH 06/38] ref(behavior_tree_plugin, traffic_simulator): separate
 pose::moveAlongLanelet

---
 .../behavior_tree_plugin/src/action_node.cpp  | 72 ++++---------------
 .../include/traffic_simulator/utils/pose.hpp  |  5 ++
 .../traffic_simulator/src/utils/pose.cpp      | 58 +++++++++++++++
 3 files changed, 75 insertions(+), 60 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 7048131138e..5f41ee43a11 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -519,26 +519,6 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   using math::geometry::operator+=;
   static bool is_yaw_applied = false;
 
-  // 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);
-  };
-
-  // 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 =
     traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner(
       step_time, canonicalized_entity_status->getName());
@@ -548,52 +528,24 @@ 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 = 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;
-
-  // 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;
-
-      // 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))
-                            : 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()) {
-    displacement = math::geometry::normalize(pose_new.orientation) * displacement;
-    pose_new.position += displacement;
+  geometry_msgs::msg::Pose pose_new;
+  if (
+    const auto canonicalized_lanelet_pose =
+      canonicalized_entity_status->getCanonicalizedLaneletPose()) {
+    pose_new = traffic_simulator::pose::moveAlongLanelet(
+      canonicalized_lanelet_pose.value(), desired_velocity, step_time, !is_yaw_applied,
+      hdmap_utils);
+    is_yaw_applied = true;
   } else {
-    const auto current_lanelet_Id = canonicalized_entity_status->getLaneletId();
-    const auto lanelet_pose_s = canonicalized_entity_status->getLaneletPose().s;
-    const auto remaining_lanelet_length =
-      hdmap_utils->getLaneletLength(current_lanelet_Id) - lanelet_pose_s;
-
-    applyPitchRotation(displacement, lanelet_pose_s);
-    applyYawRotation(displacement);
-
-    // 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(
-        remaining_lanelet_length,
-        next_lanelet_ids.empty() ? std::nullopt : std::make_optional(next_lanelet_ids[0]));
-    } else {
-      pose_new.position += displacement;
-    }
+    pose_new = canonicalized_entity_status->getMapPose();
+    pose_new.position +=
+      math::geometry::normalize(pose_new.orientation) * desired_velocity * step_time;
+    /// @todo orientation does not change?
   }
 
   auto entity_status_updated =
diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
index 9dc3b8053f8..cc914de3e28 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
@@ -60,6 +60,11 @@ auto transformRelativePoseToGlobal(
   const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose)
   -> geometry_msgs::msg::Pose;
 
+auto moveAlongLanelet(
+  const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
+  const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose;
+  
 // Relative msg::Pose
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
   -> std::optional<geometry_msgs::msg::Pose>;
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index b354ea0ea0f..6c577abfc48 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -13,6 +13,11 @@
 // limitations under the License.
 
 #include <geometry/bounding_box.hpp>
+#include <geometry/quaternion/normalize.hpp>
+#include <geometry/quaternion/quaternion_to_euler.hpp>
+#include <geometry/vector3/normalize.hpp>
+#include <geometry/vector3/operator.hpp>
+#include <geometry/vector3/rotate.hpp>
 #include <traffic_simulator/helper/helper.hpp>
 #include <traffic_simulator/utils/distance.hpp>
 #include <traffic_simulator/utils/pose.hpp>
@@ -133,6 +138,59 @@ auto transformRelativePoseToGlobal(
   return ret;
 }
 
+/// @todo remove adjust yaw... figure out a better solution, adjust_yaw is local..
+auto moveAlongLanelet(
+  const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
+  const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose
+{
+  using math::geometry::operator*;
+  using math::geometry::operator+;
+  using math::geometry::operator-;
+  using math::geometry::operator+=;
+
+  const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
+  const auto remaining_lanelet_length =
+    hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s;
+
+  auto pose_new = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose);
+  auto displacement = desired_velocity * step_time;
+
+  // Apply pitch rotation based on the lanelet's pitch angle
+  const auto lanelet_pitch =
+    math::geometry::convertQuaternionToEulerAngle(
+      hdmap_utils_ptr->toMapOrientation(lanelet_pose.lanelet_id, lanelet_pose.s))
+      .y;
+  math::geometry::rotate(displacement, lanelet_pitch, math::geometry::Axis::Y);
+
+  // Apply yaw rotation once to avoid cumulative lateral offset errors
+  if (adjust_yaw) {
+    const auto yaw = math::geometry::convertQuaternionToEulerAngle(pose_new.orientation).z;
+    math::geometry::rotate(displacement, yaw, math::geometry::Axis::Z);
+  }
+
+  // Check if the displacement exceeds the remaining lanelet length
+  if (math::geometry::norm(displacement) > remaining_lanelet_length) {
+    const auto excess_displacement =
+      displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length;
+    /// @todo why always first next_lanelet? (route?)
+    if (const auto next_lanelet_ids = hdmap_utils_ptr->getNextLaneletIds(lanelet_pose.lanelet_id);
+        !next_lanelet_ids.empty()) {
+      // transition to next lanelet
+      const auto s = math::geometry::norm(excess_displacement);
+      pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_ids[0], lanelet_pose.s);
+      // Apply lateral offset if transitioning to the next lanelet
+      pose_new.position.y += canonicalized_lanelet_pose.getLaneletPose().offset;
+    } else {
+      pose_new.position += displacement;
+    }
+  } else {
+    pose_new.position += displacement;
+  }
+  /// @todo orientation?
+  return pose_new;
+}
+
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
   -> std::optional<geometry_msgs::msg::Pose>
 {

From 4ee1ebcd46118130d35a5137684ed8752d439d87 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 01:02:21 +0100
Subject: [PATCH 07/38] ref(traffic_simulator): separete
 pose::moveToTargetPosition

---
 .../include/traffic_simulator/utils/pose.hpp  |  8 ++-
 .../src/behavior/follow_trajectory.cpp        | 49 +++++--------------
 .../traffic_simulator/src/utils/pose.cpp      | 46 +++++++++++++++++
 3 files changed, 66 insertions(+), 37 deletions(-)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
index cc914de3e28..724e2a096e8 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
@@ -64,7 +64,13 @@ auto moveAlongLanelet(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
   const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose;
-  
+
+auto moveToTargetPosition(
+  const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
+  const geometry_msgs::msg::Point & target_position,
+  const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose;
+
 // Relative msg::Pose
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
   -> std::optional<geometry_msgs::msg::Pose>;
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index d45b02688d7..69140e1fd12 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -551,44 +551,21 @@ auto makeUpdatedStatus(
        steering.
     */
     auto updated_status = entity_status;
-    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;
+    constexpr bool adjust_yaw{false};
+    constexpr bool include_crosswalk{false};
+    if (!entity_status.lanelet_pose_valid) {
+      updated_status.pose.position += desired_velocity * step_time;
+    } else if (
+      const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
+        entity_status.pose, entity_status.bounding_box, {entity_status.lanelet_pose.lanelet_id},
+        include_crosswalk, matching_distance, hdmap_utils)) {
+      updated_status.pose = pose::moveToTargetPosition(
+        canonicalized_lanelet_pose.value(), target_position, desired_velocity, step_time,
+        adjust_yaw, hdmap_utils);
+      /// @todo is the orientation changed in moveToTargetPosition?
     } 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.position += desired_velocity * step_time;
     }
 
     updated_status.pose.orientation = [&]() {
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 6c577abfc48..35c9a15ce52 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -191,6 +191,52 @@ auto moveAlongLanelet(
   return pose_new;
 }
 
+/// @todo merge moveAlongLanelet and moveToTargetPosition or separate the common part
+auto moveToTargetPosition(
+  const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
+  const geometry_msgs::msg::Point & target_position,
+  const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose
+{
+  using math::geometry::operator*;
+  using math::geometry::operator+;
+  using math::geometry::operator-;
+  using math::geometry::operator+=;
+
+  const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
+  const auto remaining_lanelet_length =
+    hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s;
+
+  auto pose_new = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose);
+  const auto displacement = desired_velocity * step_time;
+
+  // Adjust position if displacement exceeds the current lanelet length.
+  if (math::geometry::norm(displacement) > remaining_lanelet_length) {
+    const auto excess_displacement =
+      displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length;
+    /// @todo it can throw an exception... quite offten
+    // so using just target_position and toLaneletPose is a bad idea, you need to figure out another one
+    const auto target_lanelet_pose = hdmap_utils_ptr->toLaneletPose(
+      target_position, updated_status.bounding_box, false, matching_distance);
+    if (
+      const auto next_lanelet_id = hdmap_utils_ptr->getNextLaneletOnRoute(
+        lanelet_pose.lanelet_id, target_lanelet_pose->lanelet_id)) {
+      // update position to the next lanelet
+      const auto s = math::geometry::norm(excess_displacement);
+      pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_id.value(), s);
+      // apply lateral offset if transitioning to the next lanelet
+      pose_new.position.y += lanelet_pose.offset;
+    } else {
+      // apply full displacement
+      pose_new.position = updated_status.pose.position + displacement;
+    }
+  } else {
+    pose_new.position += displacement;
+  }
+  /// @todo orientation?
+  return pose_new;
+}
+
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
   -> std::optional<geometry_msgs::msg::Pose>
 {

From f9d067e411437a062ad18a0815d3b86b8b0ed663 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 01:36:03 +0100
Subject: [PATCH 08/38] ref(traffic_simulator): fix
 pose::moveToTargetLaneletPose

---
 .../include/traffic_simulator/utils/pose.hpp  |  8 ++---
 .../src/behavior/follow_trajectory.cpp        | 30 +++++++++++--------
 .../traffic_simulator/src/utils/pose.cpp      | 20 ++++++-------
 3 files changed, 31 insertions(+), 27 deletions(-)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
index 724e2a096e8..bcba09bbd6b 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
@@ -65,11 +65,11 @@ auto moveAlongLanelet(
   const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose;
 
-auto moveToTargetPosition(
+auto moveToTargetLaneletPose(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
-  const geometry_msgs::msg::Point & target_position,
-  const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose;
+  const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity,
+  const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+  -> geometry_msgs::msg::Pose;
 
 // Relative msg::Pose
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 69140e1fd12..17b489dd52b 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -26,6 +26,7 @@
 #include <scenario_simulator_exception/exception.hpp>
 #include <traffic_simulator/behavior/follow_trajectory.hpp>
 #include <traffic_simulator/behavior/follow_waypoint_controller.hpp>
+#include <traffic_simulator/utils/pose.hpp>
 
 namespace traffic_simulator
 {
@@ -66,6 +67,8 @@ auto makeUpdatedStatus(
   using math::geometry::normalize;
   using math::geometry::truncate;
 
+  constexpr bool include_crosswalk{false};
+
   auto distance_along_lanelet =
     [&](const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double {
     if (const auto from_lanelet_pose =
@@ -551,21 +554,24 @@ auto makeUpdatedStatus(
        steering.
     */
     auto updated_status = entity_status;
+    updated_status.pose.position += desired_velocity * step_time;
 
-    constexpr bool adjust_yaw{false};
-    constexpr bool include_crosswalk{false};
-    if (!entity_status.lanelet_pose_valid) {
-      updated_status.pose.position += desired_velocity * step_time;
-    } else if (
+    // optionally overwrite pose
+    /// @todo is the orientation changed in moveToTargetLaneletPose?
+    /// @todo target_lanelet_pose can be optional... quite offten so using just target_position and toLaneletPose is a bad idea, you need to figure out another idea (not so far target but the intermediate point)
+    if (entity_status.lanelet_pose_valid) {
       const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
         entity_status.pose, entity_status.bounding_box, {entity_status.lanelet_pose.lanelet_id},
-        include_crosswalk, matching_distance, hdmap_utils)) {
-      updated_status.pose = pose::moveToTargetPosition(
-        canonicalized_lanelet_pose.value(), target_position, desired_velocity, step_time,
-        adjust_yaw, hdmap_utils);
-      /// @todo is the orientation changed in moveToTargetPosition?
-    } else {
-      updated_status.pose.position += desired_velocity * step_time;
+        include_crosswalk, matching_distance, hdmap_utils);
+
+      const auto target_lanelet_pose = hdmap_utils->toLaneletPose(
+        target_position, updated_status.bounding_box, include_crosswalk, matching_distance);
+
+      if (canonicalized_lanelet_pose && target_lanelet_pose) {
+        updated_status.pose = pose::moveToTargetLaneletPose(
+          canonicalized_lanelet_pose.value(), target_lanelet_pose.value(), desired_velocity,
+          step_time, hdmap_utils);
+      }
     }
 
     updated_status.pose.orientation = [&]() {
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 35c9a15ce52..93ce0207aa0 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -156,6 +156,7 @@ auto moveAlongLanelet(
   auto pose_new = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose);
   auto displacement = desired_velocity * step_time;
 
+  /// @todo from this - these lines are just weird
   // Apply pitch rotation based on the lanelet's pitch angle
   const auto lanelet_pitch =
     math::geometry::convertQuaternionToEulerAngle(
@@ -168,6 +169,7 @@ auto moveAlongLanelet(
     const auto yaw = math::geometry::convertQuaternionToEulerAngle(pose_new.orientation).z;
     math::geometry::rotate(displacement, yaw, math::geometry::Axis::Z);
   }
+  /// @todo to this - these lines are just weird
 
   // Check if the displacement exceeds the remaining lanelet length
   if (math::geometry::norm(displacement) > remaining_lanelet_length) {
@@ -191,12 +193,12 @@ auto moveAlongLanelet(
   return pose_new;
 }
 
-/// @todo merge moveAlongLanelet and moveToTargetPosition or separate the common part
-auto moveToTargetPosition(
+/// @todo merge moveAlongLanelet and moveToTargetLaneletPose or separate the common part
+auto moveToTargetLaneletPose(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
-  const geometry_msgs::msg::Point & target_position,
-  const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose
+  const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity,
+  const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+  -> geometry_msgs::msg::Pose
 {
   using math::geometry::operator*;
   using math::geometry::operator+;
@@ -214,13 +216,9 @@ auto moveToTargetPosition(
   if (math::geometry::norm(displacement) > remaining_lanelet_length) {
     const auto excess_displacement =
       displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length;
-    /// @todo it can throw an exception... quite offten
-    // so using just target_position and toLaneletPose is a bad idea, you need to figure out another one
-    const auto target_lanelet_pose = hdmap_utils_ptr->toLaneletPose(
-      target_position, updated_status.bounding_box, false, matching_distance);
     if (
       const auto next_lanelet_id = hdmap_utils_ptr->getNextLaneletOnRoute(
-        lanelet_pose.lanelet_id, target_lanelet_pose->lanelet_id)) {
+        lanelet_pose.lanelet_id, target_lanelet_pose.lanelet_id)) {
       // update position to the next lanelet
       const auto s = math::geometry::norm(excess_displacement);
       pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_id.value(), s);
@@ -228,7 +226,7 @@ auto moveToTargetPosition(
       pose_new.position.y += lanelet_pose.offset;
     } else {
       // apply full displacement
-      pose_new.position = updated_status.pose.position + displacement;
+      pose_new.position = pose_new.position + displacement;
     }
   } else {
     pose_new.position += displacement;

From 72eea6f3580f3bd6b53681e61b737f72ae9e1c17 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 02:28:55 +0100
Subject: [PATCH 09/38] fix(traffic_simulator): fix moveToLaneletPose in
 FollowTrajectoryAction

---
 .../include/traffic_simulator/utils/pose.hpp  |  2 +-
 .../src/behavior/follow_trajectory.cpp        | 36 +++++++++----------
 .../traffic_simulator/src/utils/pose.cpp      | 24 +++++--------
 3 files changed, 28 insertions(+), 34 deletions(-)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
index bcba09bbd6b..1152807dc91 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
@@ -65,7 +65,7 @@ auto moveAlongLanelet(
   const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose;
 
-auto moveToTargetLaneletPose(
+auto moveToLaneletPose(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
   const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity,
   const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 17b489dd52b..598ed6cccf4 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -554,25 +554,8 @@ auto makeUpdatedStatus(
        steering.
     */
     auto updated_status = entity_status;
-    updated_status.pose.position += desired_velocity * step_time;
-
-    // optionally overwrite pose
-    /// @todo is the orientation changed in moveToTargetLaneletPose?
-    /// @todo target_lanelet_pose can be optional... quite offten so using just target_position and toLaneletPose is a bad idea, you need to figure out another idea (not so far target but the intermediate point)
-    if (entity_status.lanelet_pose_valid) {
-      const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
-        entity_status.pose, entity_status.bounding_box, {entity_status.lanelet_pose.lanelet_id},
-        include_crosswalk, matching_distance, hdmap_utils);
-
-      const auto target_lanelet_pose = hdmap_utils->toLaneletPose(
-        target_position, updated_status.bounding_box, include_crosswalk, matching_distance);
 
-      if (canonicalized_lanelet_pose && target_lanelet_pose) {
-        updated_status.pose = pose::moveToTargetLaneletPose(
-          canonicalized_lanelet_pose.value(), target_lanelet_pose.value(), desired_velocity,
-          step_time, hdmap_utils);
-      }
-    }
+    updated_status.pose.position += desired_velocity * step_time;
 
     updated_status.pose.orientation = [&]() {
       if (desired_velocity.y == 0 && desired_velocity.x == 0 && desired_velocity.z == 0) {
@@ -589,6 +572,23 @@ auto makeUpdatedStatus(
       }
     }();
 
+    // optionally overwrite pose
+    /// @todo is the orientation changed in moveToLaneletPose?
+    if (entity_status.lanelet_pose_valid) {
+      const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
+        entity_status.pose, entity_status.bounding_box, {entity_status.lanelet_pose.lanelet_id},
+        include_crosswalk, matching_distance, hdmap_utils);
+
+      const auto next_lanelet_pose = hdmap_utils->toLaneletPose(
+        updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance);
+
+      if (canonicalized_lanelet_pose && next_lanelet_pose) {
+        updated_status.pose = pose::moveToLaneletPose(
+          canonicalized_lanelet_pose.value(), next_lanelet_pose.value(), desired_velocity,
+          step_time, hdmap_utils);
+      }
+    }
+
     updated_status.action_status.twist.linear.x = norm(desired_velocity);
 
     updated_status.action_status.twist.linear.y = 0;
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 93ce0207aa0..b211a2b3068 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -182,6 +182,7 @@ auto moveAlongLanelet(
       const auto s = math::geometry::norm(excess_displacement);
       pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_ids[0], lanelet_pose.s);
       // Apply lateral offset if transitioning to the next lanelet
+      /// @todo offset is not the same as y...
       pose_new.position.y += canonicalized_lanelet_pose.getLaneletPose().offset;
     } else {
       pose_new.position += displacement;
@@ -193,10 +194,10 @@ auto moveAlongLanelet(
   return pose_new;
 }
 
-/// @todo merge moveAlongLanelet and moveToTargetLaneletPose or separate the common part
-auto moveToTargetLaneletPose(
+/// @todo merge moveAlongLanelet and moveToLaneletPose or separate the common part
+auto moveToLaneletPose(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
-  const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity,
+  const LaneletPose & next_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity,
   const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
   -> geometry_msgs::msg::Pose
 {
@@ -216,18 +217,11 @@ auto moveToTargetLaneletPose(
   if (math::geometry::norm(displacement) > remaining_lanelet_length) {
     const auto excess_displacement =
       displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length;
-    if (
-      const auto next_lanelet_id = hdmap_utils_ptr->getNextLaneletOnRoute(
-        lanelet_pose.lanelet_id, target_lanelet_pose.lanelet_id)) {
-      // update position to the next lanelet
-      const auto s = math::geometry::norm(excess_displacement);
-      pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_id.value(), s);
-      // apply lateral offset if transitioning to the next lanelet
-      pose_new.position.y += lanelet_pose.offset;
-    } else {
-      // apply full displacement
-      pose_new.position = pose_new.position + displacement;
-    }
+    const auto s = math::geometry::norm(excess_displacement);
+    pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_pose.lanelet_id, s);
+    // apply lateral offset if transitioning to the next lanelet
+    /// @todo offset is not the same as y...
+    pose_new.position.y += lanelet_pose.offset;
   } else {
     pose_new.position += displacement;
   }

From bccd3c5ea43a24aff90b06738a39f0ae44e7a7b7 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 10:45:37 +0100
Subject: [PATCH 10/38] fix(behavior_tree_plugin): use moveToLaneletPose in
 calculateUpdatedEntityStatusInWorldFrame

---
 .../behavior_tree_plugin/src/action_node.cpp  | 45 +++++++++++++------
 .../src/behavior/follow_trajectory.cpp        |  5 ++-
 2 files changed, 35 insertions(+), 15 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 5f41ee43a11..4f1813538df 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -517,7 +517,11 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   using math::geometry::operator+;
   using math::geometry::operator-;
   using math::geometry::operator+=;
-  static bool is_yaw_applied = false;
+
+  const auto include_crosswalk = [](const auto & entity_type) {
+    return (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == entity_type.type) ||
+           (traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == entity_type.type);
+  }(canonicalized_entity_status->getType());
 
   const auto speed_planner =
     traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner(
@@ -528,24 +532,37 @@ 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);
-  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);
 
   geometry_msgs::msg::Pose pose_new;
+  // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s)
+  geometry_msgs::msg::Vector3 delta_rotation;
+  delta_rotation.z = twist_new.angular.z * step_time;
+  const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
+  pose_new.orientation = canonicalized_entity_status->getMapPose().orientation * delta_quaternion;
+  // apply position change
+  const Eigen::Matrix3d rotation_matix = math::geometry::getRotationMatrix(pose_new.orientation);
+  const Eigen::Vector3d translation =
+    Eigen::Vector3d(twist_new.linear.x * step_time, twist_new.linear.y * step_time, 0.0);
+  const Eigen::Vector3d delta_position_eigen = rotation_matix * translation;
+  /// @todo allow: canonicalized_entity_status->getMapPose().position + delta_position_eigen
+  geometry_msgs::msg::Vector3 delta_position;
+  delta_position.x = delta_position_eigen.x();
+  delta_position.y = delta_position_eigen.y();
+  delta_position.z = delta_position_eigen.z();
+  pose_new.position = canonicalized_entity_status->getMapPose().position + delta_position;
+
   if (
     const auto canonicalized_lanelet_pose =
       canonicalized_entity_status->getCanonicalizedLaneletPose()) {
-    pose_new = traffic_simulator::pose::moveAlongLanelet(
-      canonicalized_lanelet_pose.value(), desired_velocity, step_time, !is_yaw_applied,
-      hdmap_utils);
-    is_yaw_applied = true;
-  } else {
-    pose_new = canonicalized_entity_status->getMapPose();
-    pose_new.position +=
-      math::geometry::normalize(pose_new.orientation) * desired_velocity * step_time;
-    /// @todo orientation does not change?
+    const auto next_lanelet_pose = hdmap_utils->toLaneletPose(
+      pose_new, canonicalized_entity_status->getBoundingBox(), include_crosswalk,
+      default_matching_distance_for_lanelet_pose_calculation);
+
+    if (next_lanelet_pose) {
+      pose_new = traffic_simulator::pose::moveToLaneletPose(
+        canonicalized_lanelet_pose.value(), next_lanelet_pose.value(), twist_new.linear, step_time,
+        hdmap_utils);
+    }
   }
 
   auto entity_status_updated =
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 598ed6cccf4..bcab441bd66 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -67,7 +67,10 @@ auto makeUpdatedStatus(
   using math::geometry::normalize;
   using math::geometry::truncate;
 
-  constexpr bool include_crosswalk{false};
+  const auto include_crosswalk = [](const auto & entity_type) {
+    return (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == entity_type.type) ||
+           (traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == entity_type.type);
+  }(entity_status.type);
 
   auto distance_along_lanelet =
     [&](const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double {

From 8b9a76d55ee96b33734e0bf3a3947a21510a1d30 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 10:48:48 +0100
Subject: [PATCH 11/38] Revert "ref(geometry): add axis header, improve"

This reverts commit 26f8a6547e0c4573a9ca4738e5486335a2c9e8b4.
---
 .../geometry/include/geometry/axis/axis.hpp   | 41 --------
 .../include/geometry/polygon/polygon.hpp      |  5 +
 .../include/geometry/vector3/rotate.hpp       | 31 ++++--
 common/math/geometry/src/axis/axis.cpp        | 94 -------------------
 common/math/geometry/src/polygon/polygon.cpp  | 67 +++++++++++++
 .../test/src/polygon/test_polygon.cpp         |  2 -
 .../primitives/primitive.hpp                  |  1 -
 7 files changed, 94 insertions(+), 147 deletions(-)
 delete mode 100644 common/math/geometry/include/geometry/axis/axis.hpp
 delete mode 100644 common/math/geometry/src/axis/axis.cpp

diff --git a/common/math/geometry/include/geometry/axis/axis.hpp b/common/math/geometry/include/geometry/axis/axis.hpp
deleted file mode 100644
index e0da906d9f5..00000000000
--- a/common/math/geometry/include/geometry/axis/axis.hpp
+++ /dev/null
@@ -1,41 +0,0 @@
-
-// 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__AXIS__AXIS_HPP_
-#define GEOMETRY__AXIS__AXIS_HPP_
-
-#include <Eigen/Dense>
-#include <geometry_msgs/msg/point.hpp>
-
-namespace math
-{
-namespace geometry
-{
-enum class Axis { X = 0, Y = 1, Z = 2 };
-
-auto axisToEigenAxis(Axis axis) -> Eigen::Vector3d;
-
-auto getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
-  -> double;
-
-auto getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
-  -> double;
-
-auto filterByAxis(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
-  -> std::vector<double>;
-}  // namespace geometry
-}  // namespace math
-
-#endif  // GEOMETRY__AXIS__AXIS_HPP_
diff --git a/common/math/geometry/include/geometry/polygon/polygon.hpp b/common/math/geometry/include/geometry/polygon/polygon.hpp
index 231dae67ac6..02f0374368c 100644
--- a/common/math/geometry/include/geometry/polygon/polygon.hpp
+++ b/common/math/geometry/include/geometry/polygon/polygon.hpp
@@ -21,6 +21,11 @@ namespace math
 {
 namespace geometry
 {
+enum class Axis { X = 0, Y = 1, Z = 2 };
+double getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis);
+double getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis);
+std::vector<double> filterByAxis(
+  const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis);
 std::vector<geometry_msgs::msg::Point> get2DConvexHull(
   const std::vector<geometry_msgs::msg::Point> & points);
 }  // namespace geometry
diff --git a/common/math/geometry/include/geometry/vector3/rotate.hpp b/common/math/geometry/include/geometry/vector3/rotate.hpp
index 91c5b592bae..33648f01f83 100644
--- a/common/math/geometry/include/geometry/vector3/rotate.hpp
+++ b/common/math/geometry/include/geometry/vector3/rotate.hpp
@@ -15,7 +15,6 @@
 #ifndef GEOMETRY__VECTOR3__ROTATE_HPP_
 #define GEOMETRY__VECTOR3__ROTATE_HPP_
 
-#include <geometry/axis/axis.hpp>
 #include <geometry/vector3/is_like_vector3.hpp>
 #include <scenario_simulator_exception/exception.hpp>
 
@@ -23,6 +22,20 @@ 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>
@@ -30,15 +43,15 @@ 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.");
-  } else {
-    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();
   }
+
+  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
diff --git a/common/math/geometry/src/axis/axis.cpp b/common/math/geometry/src/axis/axis.cpp
deleted file mode 100644
index 422ddbbc36c..00000000000
--- a/common/math/geometry/src/axis/axis.cpp
+++ /dev/null
@@ -1,94 +0,0 @@
-// 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.
-
-#include <boost/geometry.hpp>
-// #include <boost/geometry/geometries/point_xy.hpp>
-// #include <boost/geometry/geometries/polygon.hpp>
-// #include <geometry/polygon/polygon.hpp>
-// #include <rclcpp/rclcpp.hpp>
-#include <geometry/axis/axis.hpp>
-#include <scenario_simulator_exception/exception.hpp>
-
-namespace math
-{
-namespace geometry
-{
-auto axisToEigenAxis(Axis axis) -> Eigen::Vector3d
-{
-  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.");
-  }
-}
-
-auto getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) -> double
-{
-  if (const auto values = filterByAxis(points, axis); values.size() == 1) {
-    return values.front();
-  } else {
-    return *std::max_element(values.begin(), values.end());
-  }
-}
-
-auto getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) -> double
-{
-  if (const auto values = filterByAxis(points, axis); values.size() == 1) {
-    return values.front();
-  } else {
-    return *std::min_element(values.begin(), values.end());
-  }
-}
-
-auto filterByAxis(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
-  -> std::vector<double>
-{
-  if (points.empty()) {
-    THROW_SIMULATION_ERROR(
-      "Invalid point list is specified, while trying to filter ",
-      axis == Axis::X   ? "X"
-      : axis == Axis::Y ? "Y"
-                        : "Z",
-      " axis. ",
-      "The point list in filterByAxis() should have at least one point to filter. "
-      "This message is not originally intended to be displayed, if you see it, please "
-      "contact the developer of traffic_simulator.");
-  } else {
-    const auto axisExtractor =
-      [](const Axis axis) -> std::function<double(const geometry_msgs::msg::Point &)> {
-      switch (axis) {
-        case Axis::X:
-          return [](const geometry_msgs::msg::Point & p) { return p.x; };
-        case Axis::Y:
-          return [](const geometry_msgs::msg::Point & p) { return p.y; };
-        case Axis::Z:
-          return [](const geometry_msgs::msg::Point & p) { return p.z; };
-        default:
-          throw std::invalid_argument("Invalid axis");
-      }
-    };
-
-    std::vector<double> single_axis;
-    std::transform(
-      points.begin(), points.end(), std::back_inserter(single_axis), axisExtractor(axis));
-    return single_axis;
-  }
-}
-}  // namespace geometry
-}  // namespace math
diff --git a/common/math/geometry/src/polygon/polygon.cpp b/common/math/geometry/src/polygon/polygon.cpp
index 06886e6299a..7a7148b40d8 100644
--- a/common/math/geometry/src/polygon/polygon.cpp
+++ b/common/math/geometry/src/polygon/polygon.cpp
@@ -47,5 +47,72 @@ std::vector<geometry_msgs::msg::Point> get2DConvexHull(
   }
   return polygon;
 }
+
+double getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
+{
+  if (points.empty()) {
+    THROW_SIMULATION_ERROR(
+      "Invalid point list is specified, while getting max value on ",
+      axis == Axis::X   ? "X"
+      : axis == Axis::Y ? "Y"
+                        : "Z",
+      " axis. ",
+      "The point list in getMaxValue should have at least one point to get the max value from. "
+      "This message is not originally intended to be displayed, if you see it, please "
+      "contact the developer of traffic_simulator.");
+  }
+  const auto values = filterByAxis(points, axis);
+  if (values.size() == 1) {
+    return values.front();
+  }
+  return *std::max_element(values.begin(), values.end());
+}
+
+double getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
+{
+  if (points.empty()) {
+    THROW_SIMULATION_ERROR(
+      "Invalid point list is specified, while getting min value on ",
+      axis == Axis::X   ? "X"
+      : axis == Axis::Y ? "Y"
+                        : "Z",
+      " axis. ",
+      "The point list in getMinValue should have at least one point to get the min value from. "
+      "This message is not originally intended to be displayed, if you see it, please "
+      "contact the developer of traffic_simulator.");
+  }
+  const auto values = filterByAxis(points, axis);
+  if (values.size() == 1) {
+    return values.front();
+  }
+  return *std::min_element(values.begin(), values.end());
+}
+
+std::vector<double> filterByAxis(
+  const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
+{
+  std::vector<double> ret;
+  switch (axis) {
+    case Axis::X: {
+      std::transform(
+        points.begin(), points.end(), std::back_inserter(ret),
+        [](const geometry_msgs::msg::Point & point) { return point.x; });
+      break;
+    }
+    case Axis::Y: {
+      std::transform(
+        points.begin(), points.end(), std::back_inserter(ret),
+        [](const geometry_msgs::msg::Point & point) { return point.y; });
+      break;
+    }
+    case Axis::Z: {
+      std::transform(
+        points.begin(), points.end(), std::back_inserter(ret),
+        [](const geometry_msgs::msg::Point & point) { return point.z; });
+      break;
+    }
+  }
+  return ret;
+}
 }  // namespace geometry
 }  // namespace math
diff --git a/common/math/geometry/test/src/polygon/test_polygon.cpp b/common/math/geometry/test/src/polygon/test_polygon.cpp
index 9b4596e9913..e7278fa6566 100644
--- a/common/math/geometry/test/src/polygon/test_polygon.cpp
+++ b/common/math/geometry/test/src/polygon/test_polygon.cpp
@@ -14,7 +14,6 @@
 
 #include <gtest/gtest.h>
 
-#include <geometry/axis/axis.hpp>
 #include <geometry/polygon/polygon.hpp>
 #include <scenario_simulator_exception/exception.hpp>
 
@@ -42,7 +41,6 @@ TEST(Polygon, filterByAxis)
   EXPECT_DOUBLE_EQ(values_z[2], -3.0);
 }
 
-/// @todo refactor test to throw exception
 TEST(Polygon, filterByAxisEmptyVector)
 {
   std::vector<geometry_msgs::msg::Point> points;
diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp
index b9c6b9d2fb4..c514d40247e 100644
--- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp
+++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp
@@ -18,7 +18,6 @@
 #include <embree4/rtcore.h>
 
 #include <algorithm>
-#include <geometry/axis/axis.hpp>
 #include <geometry/polygon/polygon.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <optional>

From ab03b85f86adc39b0c563e00948353aea267db38 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 10:50:56 +0100
Subject: [PATCH 12/38] ref(geometry): remove rotate

---
 .../include/geometry/vector3/rotate.hpp       | 59 -------------------
 1 file changed, 59 deletions(-)
 delete mode 100644 common/math/geometry/include/geometry/vector3/rotate.hpp

diff --git a/common/math/geometry/include/geometry/vector3/rotate.hpp b/common/math/geometry/include/geometry/vector3/rotate.hpp
deleted file mode 100644
index 33648f01f83..00000000000
--- a/common/math/geometry/include/geometry/vector3/rotate.hpp
+++ /dev/null
@@ -1,59 +0,0 @@
-// 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_

From 0977d80a76be8771c404f9cab37c0b0618a39500 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 11:10:34 +0100
Subject: [PATCH 13/38] ref(traffic_simulator): cleanup after removal of
 pose::moveAlongLanelet

---
 .../behavior_tree_plugin/src/action_node.cpp  |  6 +-
 .../hdmap_utils/hdmap_utils.hpp               |  7 --
 .../include/traffic_simulator/utils/pose.hpp  |  5 --
 .../src/hdmap_utils/hdmap_utils.cpp           | 21 ------
 .../traffic_simulator/src/utils/pose.cpp      | 69 ++-----------------
 5 files changed, 8 insertions(+), 100 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 4f1813538df..20fe20c6806 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -18,10 +18,8 @@
 #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 <geometry/vector3/rotate.hpp>
 #include <memory>
 #include <optional>
 #include <rclcpp/rclcpp.hpp>
@@ -540,10 +538,10 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
   pose_new.orientation = canonicalized_entity_status->getMapPose().orientation * delta_quaternion;
   // apply position change
-  const Eigen::Matrix3d rotation_matix = math::geometry::getRotationMatrix(pose_new.orientation);
+  const Eigen::Matrix3d rotation_matrix = math::geometry::getRotationMatrix(pose_new.orientation);
   const Eigen::Vector3d translation =
     Eigen::Vector3d(twist_new.linear.x * step_time, twist_new.linear.y * step_time, 0.0);
-  const Eigen::Vector3d delta_position_eigen = rotation_matix * translation;
+  const Eigen::Vector3d delta_position_eigen = rotation_matrix * translation;
   /// @todo allow: canonicalized_entity_status->getMapPose().position + delta_position_eigen
   geometry_msgs::msg::Vector3 delta_position;
   delta_position.x = delta_position_eigen.x();
diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
index 9b804fd8290..f88afe67e5d 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
@@ -162,16 +162,9 @@ 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,
diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
index 1152807dc91..4982a2395c1 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
@@ -60,11 +60,6 @@ auto transformRelativePoseToGlobal(
   const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose)
   -> geometry_msgs::msg::Pose;
 
-auto moveAlongLanelet(
-  const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
-  const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose;
-
 auto moveToLaneletPose(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
   const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity,
diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
index de6e9c92f68..b5c91645ce9 100644
--- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
+++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
@@ -1098,33 +1098,12 @@ 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;
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index b211a2b3068..970fd83ccad 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -13,11 +13,9 @@
 // limitations under the License.
 
 #include <geometry/bounding_box.hpp>
-#include <geometry/quaternion/normalize.hpp>
-#include <geometry/quaternion/quaternion_to_euler.hpp>
+#include <geometry/vector3/norm.hpp>
 #include <geometry/vector3/normalize.hpp>
 #include <geometry/vector3/operator.hpp>
-#include <geometry/vector3/rotate.hpp>
 #include <traffic_simulator/helper/helper.hpp>
 #include <traffic_simulator/utils/distance.hpp>
 #include <traffic_simulator/utils/pose.hpp>
@@ -138,62 +136,6 @@ auto transformRelativePoseToGlobal(
   return ret;
 }
 
-/// @todo remove adjust yaw... figure out a better solution, adjust_yaw is local..
-auto moveAlongLanelet(
-  const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
-  const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose
-{
-  using math::geometry::operator*;
-  using math::geometry::operator+;
-  using math::geometry::operator-;
-  using math::geometry::operator+=;
-
-  const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
-  const auto remaining_lanelet_length =
-    hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s;
-
-  auto pose_new = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose);
-  auto displacement = desired_velocity * step_time;
-
-  /// @todo from this - these lines are just weird
-  // Apply pitch rotation based on the lanelet's pitch angle
-  const auto lanelet_pitch =
-    math::geometry::convertQuaternionToEulerAngle(
-      hdmap_utils_ptr->toMapOrientation(lanelet_pose.lanelet_id, lanelet_pose.s))
-      .y;
-  math::geometry::rotate(displacement, lanelet_pitch, math::geometry::Axis::Y);
-
-  // Apply yaw rotation once to avoid cumulative lateral offset errors
-  if (adjust_yaw) {
-    const auto yaw = math::geometry::convertQuaternionToEulerAngle(pose_new.orientation).z;
-    math::geometry::rotate(displacement, yaw, math::geometry::Axis::Z);
-  }
-  /// @todo to this - these lines are just weird
-
-  // Check if the displacement exceeds the remaining lanelet length
-  if (math::geometry::norm(displacement) > remaining_lanelet_length) {
-    const auto excess_displacement =
-      displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length;
-    /// @todo why always first next_lanelet? (route?)
-    if (const auto next_lanelet_ids = hdmap_utils_ptr->getNextLaneletIds(lanelet_pose.lanelet_id);
-        !next_lanelet_ids.empty()) {
-      // transition to next lanelet
-      const auto s = math::geometry::norm(excess_displacement);
-      pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_ids[0], lanelet_pose.s);
-      // Apply lateral offset if transitioning to the next lanelet
-      /// @todo offset is not the same as y...
-      pose_new.position.y += canonicalized_lanelet_pose.getLaneletPose().offset;
-    } else {
-      pose_new.position += displacement;
-    }
-  } else {
-    pose_new.position += displacement;
-  }
-  /// @todo orientation?
-  return pose_new;
-}
-
 /// @todo merge moveAlongLanelet and moveToLaneletPose or separate the common part
 auto moveToLaneletPose(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
@@ -213,12 +155,13 @@ auto moveToLaneletPose(
   auto pose_new = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose);
   const auto displacement = desired_velocity * step_time;
 
-  // Adjust position if displacement exceeds the current lanelet length.
+  // adjust position if displacement exceeds the current lanelet length
   if (math::geometry::norm(displacement) > remaining_lanelet_length) {
-    const auto excess_displacement =
+    const auto next_lanelet_displacement =
       displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length;
-    const auto s = math::geometry::norm(excess_displacement);
-    pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_pose.lanelet_id, s);
+    const auto next_lanelet_s = math::geometry::norm(next_lanelet_displacement);
+    pose_new.position =
+      hdmap_utils_ptr->toMapPosition(next_lanelet_pose.lanelet_id, next_lanelet_s);
     // apply lateral offset if transitioning to the next lanelet
     /// @todo offset is not the same as y...
     pose_new.position.y += lanelet_pose.offset;

From 5dfaf19a49747aec0db4b2426fa016fa737f6d66 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 13:17:11 +0100
Subject: [PATCH 14/38] feat(traffic_simulator): improve moveTowardsLaneletPose
 to calc LaneletPose and return it

---
 .../include/geometry/vector3/operator.hpp     | 12 +++++
 .../behavior_tree_plugin/src/action_node.cpp  | 27 ++++++-----
 .../include/traffic_simulator/utils/pose.hpp  |  4 +-
 .../src/behavior/follow_trajectory.cpp        | 17 ++++---
 .../traffic_simulator/src/utils/pose.cpp      | 46 +++++++++++--------
 5 files changed, 67 insertions(+), 39 deletions(-)

diff --git a/common/math/geometry/include/geometry/vector3/operator.hpp b/common/math/geometry/include/geometry/vector3/operator.hpp
index f1d839b6cb1..79afece710a 100644
--- a/common/math/geometry/include/geometry/vector3/operator.hpp
+++ b/common/math/geometry/include/geometry/vector3/operator.hpp
@@ -15,6 +15,7 @@
 #ifndef GEOMETRY__VECTOR3__OPERATOR_HPP_
 #define GEOMETRY__VECTOR3__OPERATOR_HPP_
 
+#include <Eigen/Dense>
 #include <geometry/vector3/is_like_vector3.hpp>
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
@@ -24,6 +25,17 @@ namespace math
 {
 namespace geometry
 {
+template <
+  typename T, std::enable_if_t<std::conjunction_v<IsLikeVector3<T>>, std::nullptr_t> = nullptr>
+auto operator+(const T & v, const Eigen::Vector3d & eigen_v) -> T
+{
+  T result;
+  result.x = v.x + eigen_v.x();
+  result.y = v.y + eigen_v.y();
+  result.z = v.z + eigen_v.z();
+  return result;
+}
+
 template <
   typename T, typename U,
   std::enable_if_t<std::conjunction_v<IsLikeVector3<T>, IsLikeVector3<U>>, std::nullptr_t> =
diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 20fe20c6806..0cb29b25276 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -19,6 +19,7 @@
 #include <geometry/quaternion/get_rotation.hpp>
 #include <geometry/quaternion/get_rotation_matrix.hpp>
 #include <geometry/quaternion/quaternion_to_euler.hpp>
+#include <geometry/vector3/hypot.hpp>
 #include <geometry/vector3/normalize.hpp>
 #include <memory>
 #include <optional>
@@ -541,25 +542,29 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   const Eigen::Matrix3d rotation_matrix = math::geometry::getRotationMatrix(pose_new.orientation);
   const Eigen::Vector3d translation =
     Eigen::Vector3d(twist_new.linear.x * step_time, twist_new.linear.y * step_time, 0.0);
-  const Eigen::Vector3d delta_position_eigen = rotation_matrix * translation;
-  /// @todo allow: canonicalized_entity_status->getMapPose().position + delta_position_eigen
-  geometry_msgs::msg::Vector3 delta_position;
-  delta_position.x = delta_position_eigen.x();
-  delta_position.y = delta_position_eigen.y();
-  delta_position.z = delta_position_eigen.z();
+  const Eigen::Vector3d delta_position = rotation_matrix * translation;
   pose_new.position = canonicalized_entity_status->getMapPose().position + delta_position;
 
+  // if the transition between lanelet pose: optionally overwrite position
   if (
     const auto canonicalized_lanelet_pose =
       canonicalized_entity_status->getCanonicalizedLaneletPose()) {
-    const auto next_lanelet_pose = hdmap_utils->toLaneletPose(
+    const auto estimated_next_lanelet_pose = hdmap_utils->toLaneletPose(
       pose_new, canonicalized_entity_status->getBoundingBox(), include_crosswalk,
       default_matching_distance_for_lanelet_pose_calculation);
 
-    if (next_lanelet_pose) {
-      pose_new = traffic_simulator::pose::moveToLaneletPose(
-        canonicalized_lanelet_pose.value(), next_lanelet_pose.value(), twist_new.linear, step_time,
-        hdmap_utils);
+    // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same
+    if (estimated_next_lanelet_pose) {
+      const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose(
+        canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(), twist_new.linear,
+        step_time, hdmap_utils);
+      const auto was_position = pose_new.position;
+      pose_new.position =
+        traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils).position;
+      // if (hypot(was_position, pose_new.position) > 0.1) {
+      //   THROW_SIMULATION_ERROR(
+      //     "Position override bug by method pose::moveTowardsLaneletPose() - too much change.");
+      // }
     }
   }
 
diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
index 4982a2395c1..04ec20ccce6 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
@@ -60,11 +60,11 @@ auto transformRelativePoseToGlobal(
   const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose)
   -> geometry_msgs::msg::Pose;
 
-auto moveToLaneletPose(
+auto moveTowardsLaneletPose(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
   const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity,
   const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
-  -> geometry_msgs::msg::Pose;
+  -> LaneletPose;
 
 // Relative msg::Pose
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index bcab441bd66..a8a91f04442 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -575,20 +575,25 @@ auto makeUpdatedStatus(
       }
     }();
 
-    // optionally overwrite pose
-    /// @todo is the orientation changed in moveToLaneletPose?
+    // if the transition between lanelet pose: optionally overwrite position
     if (entity_status.lanelet_pose_valid) {
       const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
         entity_status.pose, entity_status.bounding_box, {entity_status.lanelet_pose.lanelet_id},
         include_crosswalk, matching_distance, hdmap_utils);
 
-      const auto next_lanelet_pose = hdmap_utils->toLaneletPose(
+      const auto estimated_next_lanelet_pose = hdmap_utils->toLaneletPose(
         updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance);
 
-      if (canonicalized_lanelet_pose && next_lanelet_pose) {
-        updated_status.pose = pose::moveToLaneletPose(
-          canonicalized_lanelet_pose.value(), next_lanelet_pose.value(), desired_velocity,
+      if (canonicalized_lanelet_pose && estimated_next_lanelet_pose) {
+        const auto next_lanelet_pose = pose::moveTowardsLaneletPose(
+          canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(), desired_velocity,
           step_time, hdmap_utils);
+        const auto was_position = updated_status.pose.position;
+        updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position;
+        // if (hypot(was_position, updated_status.pose.position) > 0.1) {
+        //   THROW_SIMULATION_ERROR(
+        //     "Position override bug by method pose::moveTowardsLaneletPose() - too much change.");
+        // }
       }
     }
 
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 970fd83ccad..5a7c563a570 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -136,12 +136,13 @@ auto transformRelativePoseToGlobal(
   return ret;
 }
 
-/// @todo merge moveAlongLanelet and moveToLaneletPose or separate the common part
-auto moveToLaneletPose(
+/// @note this function does not modify the orientation of LaneletPose
+// the orientation remains the same as in next_lanelet_pose
+auto moveTowardsLaneletPose(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
   const LaneletPose & next_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity,
   const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
-  -> geometry_msgs::msg::Pose
+  -> LaneletPose
 {
   using math::geometry::operator*;
   using math::geometry::operator+;
@@ -149,27 +150,32 @@ auto moveToLaneletPose(
   using math::geometry::operator+=;
 
   const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
+  const auto yaw_relative_to_lanelet = lanelet_pose.rpy.z;
+  const auto longitudinal_d = (desired_velocity.x * cos(yaw_relative_to_lanelet) -
+                               desired_velocity.y * sin(yaw_relative_to_lanelet)) *
+                              step_time;
+  const auto lateral_d = (desired_velocity.x * sin(yaw_relative_to_lanelet) +
+                          desired_velocity.y * cos(yaw_relative_to_lanelet)) *
+                         step_time;
+
+  LaneletPose result_lanelet_pose;
   const auto remaining_lanelet_length =
     hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s;
-
-  auto pose_new = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose);
-  const auto displacement = desired_velocity * step_time;
-
-  // adjust position if displacement exceeds the current lanelet length
-  if (math::geometry::norm(displacement) > remaining_lanelet_length) {
-    const auto next_lanelet_displacement =
-      displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length;
-    const auto next_lanelet_s = math::geometry::norm(next_lanelet_displacement);
-    pose_new.position =
-      hdmap_utils_ptr->toMapPosition(next_lanelet_pose.lanelet_id, next_lanelet_s);
-    // apply lateral offset if transitioning to the next lanelet
-    /// @todo offset is not the same as y...
-    pose_new.position.y += lanelet_pose.offset;
+  const auto next_lanelet_longitudinal_d = longitudinal_d - remaining_lanelet_length;
+  if (longitudinal_d < remaining_lanelet_length) {
+    result_lanelet_pose.lanelet_id = lanelet_pose.lanelet_id;
+    result_lanelet_pose.s = lanelet_pose.s + longitudinal_d;
+    result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
+  } else if (  // if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible
+    next_lanelet_longitudinal_d < hdmap_utils_ptr->getLaneletLength(next_lanelet_pose.lanelet_id)) {
+    result_lanelet_pose.lanelet_id = next_lanelet_pose.lanelet_id;
+    result_lanelet_pose.s = next_lanelet_longitudinal_d;
+    result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
   } else {
-    pose_new.position += displacement;
+    THROW_SIMULATION_ERROR("Next lanelet is too short.");
   }
-  /// @todo orientation?
-  return pose_new;
+  result_lanelet_pose.rpy = lanelet_pose.rpy;
+  return result_lanelet_pose;
 }
 
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)

From c1df2c549833468978afed173294420cf5bed672 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 13:19:46 +0100
Subject: [PATCH 15/38] ref(traffic_simulator): remove irrelevant toMapPosition

---
 .../include/traffic_simulator/hdmap_utils/hdmap_utils.hpp   | 3 ---
 .../traffic_simulator/src/hdmap_utils/hdmap_utils.cpp       | 6 ------
 2 files changed, 9 deletions(-)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
index f88afe67e5d..9be623a2cab 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
@@ -162,9 +162,6 @@ class HdMapUtils
 
   auto getAltitude(const traffic_simulator_msgs::msg::LaneletPose &) const -> double;
 
-  auto toMapPosition(const lanelet::Id lanelet_id, const double s) const
-    -> geometry_msgs::msg::Point;
-
   auto getLaneChangeTrajectory(
     const geometry_msgs::msg::Pose & from,
     const traffic_simulator::lane_change::Parameter & lane_change_parameter,
diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
index b5c91645ce9..eab04aaf2f2 100644
--- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
+++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
@@ -1098,12 +1098,6 @@ auto HdMapUtils::getNextLaneletIds(
   return sortAndUnique(ids);
 }
 
-auto HdMapUtils::toMapPosition(const lanelet::Id lanelet_id, const double s) const
-  -> geometry_msgs::msg::Point
-{
-  return getCenterPointsSpline(lanelet_id)->getPoint(s);
-}
-
 auto HdMapUtils::getTrafficLightIds() const -> lanelet::Ids
 {
   using namespace lanelet::utils::query;

From 14098e974f96c91c1eeaa162f3e867d72c5b5d22 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 14:16:49 +0100
Subject: [PATCH 16/38] fix(traffic_simulator): fix moveTowardsLaneletPose

---
 .../behavior_tree_plugin/src/action_node.cpp  |  8 +++----
 .../src/behavior/follow_trajectory.cpp        |  8 +++----
 .../traffic_simulator/src/utils/pose.cpp      | 23 +++++++++++--------
 3 files changed, 22 insertions(+), 17 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 0cb29b25276..4abb5f647dc 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -561,10 +561,10 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
       const auto was_position = pose_new.position;
       pose_new.position =
         traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils).position;
-      // if (hypot(was_position, pose_new.position) > 0.1) {
-      //   THROW_SIMULATION_ERROR(
-      //     "Position override bug by method pose::moveTowardsLaneletPose() - too much change.");
-      // }
+      if (math::geometry::hypot(was_position, pose_new.position) > 0.1) {
+        THROW_SIMULATION_ERROR(
+          "Position override bug by method pose::moveTowardsLaneletPose() - too much change.");
+      }
     }
   }
 
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index a8a91f04442..a72ff2e3a95 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -590,10 +590,10 @@ auto makeUpdatedStatus(
           step_time, hdmap_utils);
         const auto was_position = updated_status.pose.position;
         updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position;
-        // if (hypot(was_position, updated_status.pose.position) > 0.1) {
-        //   THROW_SIMULATION_ERROR(
-        //     "Position override bug by method pose::moveTowardsLaneletPose() - too much change.");
-        // }
+        if (hypot(was_position, updated_status.pose.position) > 0.1) {
+          THROW_SIMULATION_ERROR(
+            "Position override bug by method pose::moveTowardsLaneletPose() - too much change.");
+        }
       }
     }
 
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 5a7c563a570..302a6a00e1e 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -150,13 +150,19 @@ auto moveTowardsLaneletPose(
   using math::geometry::operator+=;
 
   const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
-  const auto yaw_relative_to_lanelet = lanelet_pose.rpy.z;
-  const auto longitudinal_d = (desired_velocity.x * cos(yaw_relative_to_lanelet) -
-                               desired_velocity.y * sin(yaw_relative_to_lanelet)) *
-                              step_time;
-  const auto lateral_d = (desired_velocity.x * sin(yaw_relative_to_lanelet) +
-                          desired_velocity.y * cos(yaw_relative_to_lanelet)) *
-                         step_time;
+
+  // transform desired (global) velocity to local velocity
+  const auto orientation =
+    static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose).orientation;
+  const Eigen::Vector3d global_velocity(desired_velocity.x, desired_velocity.y, desired_velocity.z);
+  const Eigen::Quaterniond quaternion(orientation.w, orientation.x, orientation.y, orientation.z);
+  const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity;
+  // determine the displacement in the 2D lanelet coordinate system
+  const Eigen::Vector2d displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
+                                       Eigen::Vector2d(local_velocity.x(), local_velocity.y()) *
+                                       step_time;
+  const auto longitudinal_d = displacement.x();
+  const auto lateral_d = displacement.y();
 
   LaneletPose result_lanelet_pose;
   const auto remaining_lanelet_length =
@@ -165,15 +171,14 @@ auto moveTowardsLaneletPose(
   if (longitudinal_d < remaining_lanelet_length) {
     result_lanelet_pose.lanelet_id = lanelet_pose.lanelet_id;
     result_lanelet_pose.s = lanelet_pose.s + longitudinal_d;
-    result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
   } else if (  // if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible
     next_lanelet_longitudinal_d < hdmap_utils_ptr->getLaneletLength(next_lanelet_pose.lanelet_id)) {
     result_lanelet_pose.lanelet_id = next_lanelet_pose.lanelet_id;
     result_lanelet_pose.s = next_lanelet_longitudinal_d;
-    result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
   } else {
     THROW_SIMULATION_ERROR("Next lanelet is too short.");
   }
+  result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
   result_lanelet_pose.rpy = lanelet_pose.rpy;
   return result_lanelet_pose;
 }

From 311f82a25de3aed1eb4328aa974345c2e561a2fc Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 14:31:35 +0100
Subject: [PATCH 17/38] ref(geometry): remove confusing operator*(quat,vec)

---
 .../include/geometry/quaternion/operator.hpp  | 28 -------------------
 1 file changed, 28 deletions(-)

diff --git a/common/math/geometry/include/geometry/quaternion/operator.hpp b/common/math/geometry/include/geometry/quaternion/operator.hpp
index 4839a282333..848e1d8332c 100644
--- a/common/math/geometry/include/geometry/quaternion/operator.hpp
+++ b/common/math/geometry/include/geometry/quaternion/operator.hpp
@@ -65,34 +65,6 @@ 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 & rotation, const U & vector)
-{
-  T vector_as_quaternion;
-  vector_as_quaternion.x = vector.x;
-  vector_as_quaternion.y = vector.y;
-  vector_as_quaternion.z = vector.z;
-  vector_as_quaternion.w = 0.0;
-
-  T inverse_rotation = rotation;
-  inverse_rotation.x = -rotation.x;
-  inverse_rotation.y = -rotation.y;
-  inverse_rotation.z = -rotation.z;
-  inverse_rotation.w = rotation.w;
-
-  T rotated_quaternion = rotation * vector_as_quaternion * inverse_rotation;
-
-  U rotated_vector;
-  rotated_vector.x = vector_as_quaternion.x;
-  rotated_vector.y = vector_as_quaternion.y;
-  rotated_vector.z = vector_as_quaternion.z;
-
-  return rotated_vector;
-}
-
 template <
   typename T, typename U,
   std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>, IsLikeQuaternion<U>>, std::nullptr_t> =

From e3df32007790fbf1e0e30eb14cafe5cb44353caf Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 15:05:43 +0100
Subject: [PATCH 18/38] ref(behavior_tree_plugin): refactor
 ActionNode::calculateUpdatedEntityStatusInWorldFrame

---
 .../include/geometry/quaternion/operator.hpp  |  1 -
 .../behavior_tree_plugin/src/action_node.cpp  | 86 ++++++++++---------
 2 files changed, 47 insertions(+), 40 deletions(-)

diff --git a/common/math/geometry/include/geometry/quaternion/operator.hpp b/common/math/geometry/include/geometry/quaternion/operator.hpp
index 848e1d8332c..624dd686852 100644
--- a/common/math/geometry/include/geometry/quaternion/operator.hpp
+++ b/common/math/geometry/include/geometry/quaternion/operator.hpp
@@ -16,7 +16,6 @@
 #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
diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 4abb5f647dc..0ccdbc78622 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -522,51 +522,59 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
            (traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == entity_type.type);
   }(canonicalized_entity_status->getType());
 
+  const auto matching_distance = default_matching_distance_for_lanelet_pose_calculation;
+
+  const auto buildUpdatedPose =
+    [&include_crosswalk, &matching_distance](
+      const std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> & status,
+      const geometry_msgs::msg::Twist & desired_twist, const double step_time,
+      const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+    -> geometry_msgs::msg::Pose {
+    geometry_msgs::msg::Pose updated_pose;
+
+    // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s)
+    geometry_msgs::msg::Vector3 delta_rotation;
+    delta_rotation.z = desired_twist.angular.z * step_time;
+    const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
+    updated_pose.orientation = status->getMapPose().orientation * delta_quaternion;
+
+    // apply position change
+    const Eigen::Matrix3d rotation_matrix =
+      math::geometry::getRotationMatrix(updated_pose.orientation);
+    const Eigen::Vector3d translation = Eigen::Vector3d(
+      desired_twist.linear.x * step_time, desired_twist.linear.y * step_time,
+      desired_twist.linear.z * step_time);
+    const Eigen::Vector3d delta_position = rotation_matrix * translation;
+    updated_pose.position = status->getMapPose().position + delta_position;
+
+    // if it is the transition between lanelet pose: optionally overwrite position to improve precision
+    if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
+      const auto estimated_next_lanelet_pose = hdmap_utils_ptr->toLaneletPose(
+        updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance);
+
+      // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same
+      if (estimated_next_lanelet_pose) {
+        const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose(
+          canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(),
+          desired_twist.linear, step_time, hdmap_utils_ptr);
+        updated_pose.position =
+          traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position;
+      }
+    }
+    return updated_pose;
+  };
+
   const auto speed_planner =
     traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner(
       step_time, canonicalized_entity_status->getName());
   const auto dynamics = speed_planner.getDynamicStates(
     target_speed, constraints, canonicalized_entity_status->getTwist(),
     canonicalized_entity_status->getAccel());
-  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;
-  // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s)
-  geometry_msgs::msg::Vector3 delta_rotation;
-  delta_rotation.z = twist_new.angular.z * step_time;
-  const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
-  pose_new.orientation = canonicalized_entity_status->getMapPose().orientation * delta_quaternion;
-  // apply position change
-  const Eigen::Matrix3d rotation_matrix = math::geometry::getRotationMatrix(pose_new.orientation);
-  const Eigen::Vector3d translation =
-    Eigen::Vector3d(twist_new.linear.x * step_time, twist_new.linear.y * step_time, 0.0);
-  const Eigen::Vector3d delta_position = rotation_matrix * translation;
-  pose_new.position = canonicalized_entity_status->getMapPose().position + delta_position;
-
-  // if the transition between lanelet pose: optionally overwrite position
-  if (
-    const auto canonicalized_lanelet_pose =
-      canonicalized_entity_status->getCanonicalizedLaneletPose()) {
-    const auto estimated_next_lanelet_pose = hdmap_utils->toLaneletPose(
-      pose_new, canonicalized_entity_status->getBoundingBox(), include_crosswalk,
-      default_matching_distance_for_lanelet_pose_calculation);
-
-    // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same
-    if (estimated_next_lanelet_pose) {
-      const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose(
-        canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(), twist_new.linear,
-        step_time, hdmap_utils);
-      const auto was_position = pose_new.position;
-      pose_new.position =
-        traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils).position;
-      if (math::geometry::hypot(was_position, pose_new.position) > 0.1) {
-        THROW_SIMULATION_ERROR(
-          "Position override bug by method pose::moveTowardsLaneletPose() - too much change.");
-      }
-    }
-  }
+  const auto linear_jerk_new = std::get<2>(dynamics);
+  const auto accel_new = std::get<1>(dynamics);
+  const auto twist_new = std::get<0>(dynamics);
+  const auto pose_new =
+    buildUpdatedPose(canonicalized_entity_status, twist_new, step_time, hdmap_utils);
 
   auto entity_status_updated =
     static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status);

From 4efd379f5ae52cbba916e0c7325ec1f99d7cd7b0 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 15:13:08 +0100
Subject: [PATCH 19/38] ref(behavior_tree_plugin, traffic_simulator): improve
 comments

---
 simulation/behavior_tree_plugin/src/action_node.cpp        | 5 ++---
 .../traffic_simulator/src/behavior/follow_trajectory.cpp   | 7 +------
 2 files changed, 3 insertions(+), 9 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 0ccdbc78622..5e7c404c270 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -19,8 +19,7 @@
 #include <geometry/quaternion/get_rotation.hpp>
 #include <geometry/quaternion/get_rotation_matrix.hpp>
 #include <geometry/quaternion/quaternion_to_euler.hpp>
-#include <geometry/vector3/hypot.hpp>
-#include <geometry/vector3/normalize.hpp>
+#include <geometry/vector3/operator.hpp>
 #include <memory>
 #include <optional>
 #include <rclcpp/rclcpp.hpp>
@@ -547,7 +546,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
     const Eigen::Vector3d delta_position = rotation_matrix * translation;
     updated_pose.position = status->getMapPose().position + delta_position;
 
-    // if it is the transition between lanelet pose: optionally overwrite position to improve precision
+    // if it is the transition between lanelet pose: overwrite position to improve precision
     if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
       const auto estimated_next_lanelet_pose = hdmap_utils_ptr->toLaneletPose(
         updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance);
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index a72ff2e3a95..029a07007dc 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -575,7 +575,7 @@ auto makeUpdatedStatus(
       }
     }();
 
-    // if the transition between lanelet pose: optionally overwrite position
+    // if it is the transition between lanelet pose: overwrite position to improve precision
     if (entity_status.lanelet_pose_valid) {
       const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
         entity_status.pose, entity_status.bounding_box, {entity_status.lanelet_pose.lanelet_id},
@@ -588,12 +588,7 @@ auto makeUpdatedStatus(
         const auto next_lanelet_pose = pose::moveTowardsLaneletPose(
           canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(), desired_velocity,
           step_time, hdmap_utils);
-        const auto was_position = updated_status.pose.position;
         updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position;
-        if (hypot(was_position, updated_status.pose.position) > 0.1) {
-          THROW_SIMULATION_ERROR(
-            "Position override bug by method pose::moveTowardsLaneletPose() - too much change.");
-        }
       }
     }
 

From c59228c5c30fb7cc44048dd9df1be0d83a6dd57a Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 19 Dec 2024 16:06:59 +0100
Subject: [PATCH 20/38] fix(traffis_simulator): use next canonicalized lanelet
 pose in pose::moveTowardsLaneletPose

---
 .../behavior_tree_plugin/src/action_node.cpp  | 10 ++++++----
 .../include/traffic_simulator/utils/pose.hpp  |  6 +++---
 .../src/behavior/follow_trajectory.cpp        | 19 ++++++++++++-------
 .../traffic_simulator/src/utils/pose.cpp      | 16 ++++++++++------
 4 files changed, 31 insertions(+), 20 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 5e7c404c270..9928346b1ae 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -548,13 +548,15 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
 
     // if it is the transition between lanelet pose: overwrite position to improve precision
     if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
-      const auto estimated_next_lanelet_pose = hdmap_utils_ptr->toLaneletPose(
-        updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance);
+      const auto estimated_next_canonicalized_lanelet_pose =
+        traffic_simulator::pose::toCanonicalizedLaneletPose(
+          updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance,
+          hdmap_utils_ptr);
 
       // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same
-      if (estimated_next_lanelet_pose) {
+      if (estimated_next_canonicalized_lanelet_pose) {
         const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose(
-          canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(),
+          canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
           desired_twist.linear, step_time, hdmap_utils_ptr);
         updated_pose.position =
           traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position;
diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
index 04ec20ccce6..5d6cceafb4a 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
@@ -62,9 +62,9 @@ auto transformRelativePoseToGlobal(
 
 auto moveTowardsLaneletPose(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
-  const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity,
-  const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
-  -> LaneletPose;
+  const CanonicalizedLaneletPose & next_canonicalized_lanelet_pose,
+  const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> LaneletPose;
 
 // Relative msg::Pose
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 029a07007dc..d95e21889ae 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -558,6 +558,8 @@ auto makeUpdatedStatus(
     */
     auto updated_status = entity_status;
 
+    updated_status.lanelet_pose_valid = false;
+
     updated_status.pose.position += desired_velocity * step_time;
 
     updated_status.pose.orientation = [&]() {
@@ -581,14 +583,19 @@ auto makeUpdatedStatus(
         entity_status.pose, entity_status.bounding_box, {entity_status.lanelet_pose.lanelet_id},
         include_crosswalk, matching_distance, hdmap_utils);
 
-      const auto estimated_next_lanelet_pose = hdmap_utils->toLaneletPose(
-        updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance);
+      const auto estimated_next_canonicalized_lanelet_pose =
+        traffic_simulator::pose::toCanonicalizedLaneletPose(
+          updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance,
+          hdmap_utils);
 
-      if (canonicalized_lanelet_pose && estimated_next_lanelet_pose) {
+      if (canonicalized_lanelet_pose && estimated_next_canonicalized_lanelet_pose) {
         const auto next_lanelet_pose = pose::moveTowardsLaneletPose(
-          canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(), desired_velocity,
-          step_time, hdmap_utils);
+          canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
+          desired_velocity, step_time, hdmap_utils);
         updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position;
+        /// @todo uncomment to test it!
+        // updated_status.lanelet_pose = next_lanelet_pose
+        // updated_status.lanelet_pose_valid = true;
       }
     }
 
@@ -613,8 +620,6 @@ auto makeUpdatedStatus(
 
     updated_status.time = entity_status.time + step_time;
 
-    updated_status.lanelet_pose_valid = false;
-
     return updated_status;
   }
 }
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 302a6a00e1e..56df046f604 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -140,9 +140,9 @@ auto transformRelativePoseToGlobal(
 // the orientation remains the same as in next_lanelet_pose
 auto moveTowardsLaneletPose(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
-  const LaneletPose & next_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity,
-  const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
-  -> LaneletPose
+  const CanonicalizedLaneletPose & next_canonicalized_lanelet_pose,
+  const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> LaneletPose
 {
   using math::geometry::operator*;
   using math::geometry::operator+;
@@ -150,6 +150,7 @@ auto moveTowardsLaneletPose(
   using math::geometry::operator+=;
 
   const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
+  const auto next_lanelet_pose = static_cast<LaneletPose>(next_canonicalized_lanelet_pose);
 
   // transform desired (global) velocity to local velocity
   const auto orientation =
@@ -168,17 +169,20 @@ auto moveTowardsLaneletPose(
   const auto remaining_lanelet_length =
     hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s;
   const auto next_lanelet_longitudinal_d = longitudinal_d - remaining_lanelet_length;
-  if (longitudinal_d < remaining_lanelet_length) {
+  if (longitudinal_d <= remaining_lanelet_length) {
     result_lanelet_pose.lanelet_id = lanelet_pose.lanelet_id;
     result_lanelet_pose.s = lanelet_pose.s + longitudinal_d;
+    result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
   } else if (  // if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible
     next_lanelet_longitudinal_d < hdmap_utils_ptr->getLaneletLength(next_lanelet_pose.lanelet_id)) {
     result_lanelet_pose.lanelet_id = next_lanelet_pose.lanelet_id;
     result_lanelet_pose.s = next_lanelet_longitudinal_d;
+    result_lanelet_pose.offset = next_lanelet_pose.offset + lateral_d;
   } else {
-    THROW_SIMULATION_ERROR("Next lanelet is too short.");
+    THROW_SIMULATION_ERROR(
+      "Next lanelet is too short: lanelet_id==", next_lanelet_pose.lanelet_id, " is shroter than ",
+      next_lanelet_longitudinal_d);
   }
-  result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
   result_lanelet_pose.rpy = lanelet_pose.rpy;
   return result_lanelet_pose;
 }

From 4b27b38d7ff729c3583f59785393f5037fdd188e Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Fri, 20 Dec 2024 11:28:52 +0100
Subject: [PATCH 21/38] fix(traffic_simulator) Fix an issue with negative
 longitudinal displacement

---
 .../src/behavior/follow_trajectory.cpp        |  3 --
 .../traffic_simulator/src/utils/pose.cpp      | 33 +++++++++++++++++--
 2 files changed, 30 insertions(+), 6 deletions(-)

diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index d95e21889ae..38cd007f203 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -593,9 +593,6 @@ auto makeUpdatedStatus(
           canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
           desired_velocity, step_time, hdmap_utils);
         updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position;
-        /// @todo uncomment to test it!
-        // updated_status.lanelet_pose = next_lanelet_pose
-        // updated_status.lanelet_pose_valid = true;
       }
     }
 
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 56df046f604..9f3ee0e17e3 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -13,6 +13,8 @@
 // limitations under the License.
 
 #include <geometry/bounding_box.hpp>
+#include <geometry/quaternion/euler_to_quaternion.hpp>
+#include <geometry/quaternion/quaternion_to_euler.hpp>
 #include <geometry/vector3/norm.hpp>
 #include <geometry/vector3/normalize.hpp>
 #include <geometry/vector3/operator.hpp>
@@ -152,14 +154,39 @@ auto moveTowardsLaneletPose(
   const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
   const auto next_lanelet_pose = static_cast<LaneletPose>(next_canonicalized_lanelet_pose);
 
-  // transform desired (global) velocity to local velocity
+  /*
+   * When the yaw of the lanelet is outside the range -90° to +90°, 
+   * transforming the global velocity using this orientation results 
+   * in a negative X component of the local velocity. This causes a 
+   * negative longitudinal displacement, making the vehicle appear 
+   * to move backward even when the intended motion is forward.
+   * 
+   * To address this issue, we adjust the yaw of the lanelet by ±180° 
+   * if it falls outside the range -90° to +90°. This correction ensures 
+   * that the local velocity's X component remains positive, resulting 
+   * in the vehicle moving forward as intended.
+   */
   const auto orientation =
     static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose).orientation;
+  auto orientation_rpy = math::geometry::convertQuaternionToEulerAngle(orientation);
+
+  if (std::abs(orientation_rpy.z) > M_PI / 2) {
+    if (orientation_rpy.z > 0) {
+      orientation_rpy.z -= M_PI;
+    } else {
+      orientation_rpy.z += M_PI;
+    }
+  }
+
+  // transform desired (global) velocity to local velocity
+  const auto corrected_orientation = math::geometry::convertEulerAngleToQuaternion(orientation_rpy);
   const Eigen::Vector3d global_velocity(desired_velocity.x, desired_velocity.y, desired_velocity.z);
-  const Eigen::Quaterniond quaternion(orientation.w, orientation.x, orientation.y, orientation.z);
+  const Eigen::Quaterniond quaternion(
+    corrected_orientation.w, corrected_orientation.x, corrected_orientation.y,
+    corrected_orientation.z);
   const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity;
   // determine the displacement in the 2D lanelet coordinate system
-  const Eigen::Vector2d displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
+  const Eigen::Vector2d displacement = Eigen::Rotation2Dd(orientation_rpy.z) *
                                        Eigen::Vector2d(local_velocity.x(), local_velocity.y()) *
                                        step_time;
   const auto longitudinal_d = displacement.x();

From 32a88386af39c52c7e32bee48d6628f3fd6a5ff9 Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Fri, 20 Dec 2024 11:32:17 +0100
Subject: [PATCH 22/38] refactor(traffic_simulator) fix spell check issue

---
 simulation/traffic_simulator/src/utils/pose.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 9f3ee0e17e3..979cefedb2f 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -207,7 +207,7 @@ auto moveTowardsLaneletPose(
     result_lanelet_pose.offset = next_lanelet_pose.offset + lateral_d;
   } else {
     THROW_SIMULATION_ERROR(
-      "Next lanelet is too short: lanelet_id==", next_lanelet_pose.lanelet_id, " is shroter than ",
+      "Next lanelet is too short: lanelet_id==", next_lanelet_pose.lanelet_id, " is shorter than ",
       next_lanelet_longitudinal_d);
   }
   result_lanelet_pose.rpy = lanelet_pose.rpy;

From 788259ccacb017d5d5ca0ff389104fd7c8fa468f Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Fri, 20 Dec 2024 16:04:27 +0100
Subject: [PATCH 23/38] tmp(behavior_tree_plugin, traffic_simulator):
 moveTowardsLaneletPose debug

---
 .../behavior_tree_plugin/src/action_node.cpp  | 28 +++++++--
 .../follow_polyline_trajectory_action.cpp     |  5 +-
 .../include/traffic_simulator/utils/pose.hpp  |  5 +-
 .../src/behavior/follow_trajectory.cpp        |  9 ++-
 .../src/data_type/lanelet_pose.cpp            |  2 +-
 .../traffic_simulator/src/utils/pose.cpp      | 58 ++++++++-----------
 6 files changed, 61 insertions(+), 46 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 9928346b1ae..2f6beac294b 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -530,7 +530,6 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
       const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
     -> geometry_msgs::msg::Pose {
     geometry_msgs::msg::Pose updated_pose;
-
     // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s)
     geometry_msgs::msg::Vector3 delta_rotation;
     delta_rotation.z = desired_twist.angular.z * step_time;
@@ -538,6 +537,8 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
     updated_pose.orientation = status->getMapPose().orientation * delta_quaternion;
 
     // apply position change
+    /// @todo first determine global desired_velocity, calculate position change using it
+    // then pass the same global desired_velocity to moveTowardsLaneletPose()
     const Eigen::Matrix3d rotation_matrix =
       math::geometry::getRotationMatrix(updated_pose.orientation);
     const Eigen::Vector3d translation = Eigen::Vector3d(
@@ -546,22 +547,39 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
     const Eigen::Vector3d delta_position = rotation_matrix * translation;
     updated_pose.position = status->getMapPose().position + delta_position;
 
+    std::cout << " 1 " << std::endl;
     // if it is the transition between lanelet pose: overwrite position to improve precision
     if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
       const auto estimated_next_canonicalized_lanelet_pose =
         traffic_simulator::pose::toCanonicalizedLaneletPose(
           updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance,
           hdmap_utils_ptr);
-
+      const auto lanelet_pose =
+        static_cast<traffic_simulator::LaneletPose>(canonicalized_lanelet_pose.value());
+      const auto pose = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose.value());
+      std::cout << " 2 " << std::endl;
+      std::cout << " -- lanelet pose: id:" << lanelet_pose.lanelet_id << " s:" << lanelet_pose.s
+                << " offset:" << lanelet_pose.offset << " yaw: " << lanelet_pose.rpy.z << std::endl;
+      std::cout << " -- orientation: "
+                << math::geometry::convertQuaternionToEulerAngle(pose.orientation).z << std::endl;
+      std::cout << " -- d_velocity: x:" << desired_twist.linear.x << " y:" << desired_twist.linear.y
+                << " z:" << desired_twist.linear.z << std::endl;
       // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same
       if (estimated_next_canonicalized_lanelet_pose) {
         const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose(
           canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
-          desired_twist.linear, step_time, hdmap_utils_ptr);
-        updated_pose.position =
-          traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position;
+          desired_twist.linear, false, step_time, hdmap_utils_ptr);
+        std::cout << " 3 " << std::endl;
+        if (
+          const auto next_canonicalized_lanelet_pose =
+            traffic_simulator::pose::canonicalize(next_lanelet_pose, hdmap_utils_ptr)) {
+          std::cout << " 4 " << std::endl;
+          updated_pose.position =
+            static_cast<geometry_msgs::msg::Pose>(next_canonicalized_lanelet_pose.value()).position;
+        }
       }
     }
+    std::cout << " ~~~~~ " << std::endl;
     return updated_pose;
   };
 
diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
index 8e722f5afb0..9ae8d6e5889 100644
--- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
+++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
@@ -63,12 +63,13 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
       return canonicalized_entity_status->getTwist().linear.x;
     }
   };
-
+  std::cout << "FollowPolylineTrajectoryAction" << std::endl;
   if (getBlackBoardValues();
       request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
       not getInput<decltype(target_speed)>("target_speed", target_speed) or
       not polyline_trajectory) {
+    std::cout << "~~FollowPolylineTrajectoryAction" << std::endl;
     return BT::NodeStatus::FAILURE;
   } else if (std::isnan(canonicalized_entity_status->getTime())) {
     THROW_SIMULATION_ERROR(
@@ -82,8 +83,10 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
     setCanonicalizedEntityStatus(entity_status_updated.value());
     setOutput("waypoints", calculateWaypoints());
     setOutput("obstacle", calculateObstacle(calculateWaypoints()));
+    std::cout << "~~FollowPolylineTrajectoryAction" << std::endl;
     return BT::NodeStatus::RUNNING;
   } else {
+    std::cout << "~~FollowPolylineTrajectoryAction" << std::endl;
     return BT::NodeStatus::SUCCESS;
   }
 }
diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
index 5d6cceafb4a..93b13227fd2 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
@@ -63,8 +63,9 @@ auto transformRelativePoseToGlobal(
 auto moveTowardsLaneletPose(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
   const CanonicalizedLaneletPose & next_canonicalized_lanelet_pose,
-  const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> LaneletPose;
+  const geometry_msgs::msg::Vector3 & desired_velocity, const bool desired_velocity_is_global,
+  const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+  -> LaneletPose;
 
 // Relative msg::Pose
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 38cd007f203..37807afb9d1 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -591,8 +591,13 @@ auto makeUpdatedStatus(
       if (canonicalized_lanelet_pose && estimated_next_canonicalized_lanelet_pose) {
         const auto next_lanelet_pose = pose::moveTowardsLaneletPose(
           canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
-          desired_velocity, step_time, hdmap_utils);
-        updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position;
+          desired_velocity, true, step_time, hdmap_utils);
+        if (
+          const auto next_canonicalized_lanelet_pose =
+            pose::canonicalize(next_lanelet_pose, hdmap_utils)) {
+          updated_status.pose.position =
+            static_cast<geometry_msgs::msg::Pose>(next_canonicalized_lanelet_pose.value()).position;
+        }
       }
     }
 
diff --git a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp
index b4dfb4dc0d8..39b24b6c738 100644
--- a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp
+++ b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp
@@ -154,7 +154,7 @@ auto CanonicalizedLaneletPose::adjustOrientationAndOzPosition(
                                       .y(lanelet_rpy.y)
                                       .z(entity_rpy.z));
     lanelet_pose_.rpy =
-      convertQuaternionToEulerAngle(getRotation(lanelet_quaternion, map_pose_.orientation));
+      convertQuaternionToEulerAngle(getRotation(map_pose_.orientation, lanelet_quaternion));
   }
 }
 
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 979cefedb2f..69e6bada0c0 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -143,8 +143,9 @@ auto transformRelativePoseToGlobal(
 auto moveTowardsLaneletPose(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
   const CanonicalizedLaneletPose & next_canonicalized_lanelet_pose,
-  const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> LaneletPose
+  const geometry_msgs::msg::Vector3 & desired_velocity, const bool desired_velocity_is_global,
+  const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+  -> LaneletPose
 {
   using math::geometry::operator*;
   using math::geometry::operator+;
@@ -154,41 +155,28 @@ auto moveTowardsLaneletPose(
   const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
   const auto next_lanelet_pose = static_cast<LaneletPose>(next_canonicalized_lanelet_pose);
 
-  /*
-   * When the yaw of the lanelet is outside the range -90° to +90°, 
-   * transforming the global velocity using this orientation results 
-   * in a negative X component of the local velocity. This causes a 
-   * negative longitudinal displacement, making the vehicle appear 
-   * to move backward even when the intended motion is forward.
-   * 
-   * To address this issue, we adjust the yaw of the lanelet by ±180° 
-   * if it falls outside the range -90° to +90°. This correction ensures 
-   * that the local velocity's X component remains positive, resulting 
-   * in the vehicle moving forward as intended.
-   */
+  // transform desired (global) velocity to local velocity
   const auto orientation =
     static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose).orientation;
-  auto orientation_rpy = math::geometry::convertQuaternionToEulerAngle(orientation);
 
-  if (std::abs(orientation_rpy.z) > M_PI / 2) {
-    if (orientation_rpy.z > 0) {
-      orientation_rpy.z -= M_PI;
-    } else {
-      orientation_rpy.z += M_PI;
-    }
-  }
+  // const auto fi = lanelet_pose.rpy.z;
+  // const auto orientation_rpy = math::geometry::convertQuaternionToEulerAngle(orientation);
+  // std::cout << "-- lanelet_pose.rpy.z: " << lanelet_pose.rpy.z
+  //           << " | orientation_rpy.z: " << orientation_rpy.z << " | fi: " << fi << std::endl;
 
-  // transform desired (global) velocity to local velocity
-  const auto corrected_orientation = math::geometry::convertEulerAngleToQuaternion(orientation_rpy);
-  const Eigen::Vector3d global_velocity(desired_velocity.x, desired_velocity.y, desired_velocity.z);
-  const Eigen::Quaterniond quaternion(
-    corrected_orientation.w, corrected_orientation.x, corrected_orientation.y,
-    corrected_orientation.z);
-  const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity;
-  // determine the displacement in the 2D lanelet coordinate system
-  const Eigen::Vector2d displacement = Eigen::Rotation2Dd(orientation_rpy.z) *
-                                       Eigen::Vector2d(local_velocity.x(), local_velocity.y()) *
-                                       step_time;
+  Eigen::Vector2d displacement;
+  if (desired_velocity_is_global) {
+    const Eigen::Vector3d global_velocity(
+      desired_velocity.x, desired_velocity.y, desired_velocity.z);
+    const Eigen::Quaterniond quaternion(orientation.w, orientation.x, orientation.y, orientation.z);
+    const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity;
+    // determine the displacement in the 2D lanelet coordinate system
+    displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
+                   Eigen::Vector2d(local_velocity.x(), local_velocity.y()) * step_time;
+  } else {
+    displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
+                   Eigen::Vector2d(desired_velocity.x, desired_velocity.y) * step_time;
+  }
   const auto longitudinal_d = displacement.x();
   const auto lateral_d = displacement.y();
 
@@ -199,12 +187,12 @@ auto moveTowardsLaneletPose(
   if (longitudinal_d <= remaining_lanelet_length) {
     result_lanelet_pose.lanelet_id = lanelet_pose.lanelet_id;
     result_lanelet_pose.s = lanelet_pose.s + longitudinal_d;
-    result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
+    result_lanelet_pose.offset = lanelet_pose.offset;  // + lateral_d;
   } else if (  // if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible
     next_lanelet_longitudinal_d < hdmap_utils_ptr->getLaneletLength(next_lanelet_pose.lanelet_id)) {
     result_lanelet_pose.lanelet_id = next_lanelet_pose.lanelet_id;
     result_lanelet_pose.s = next_lanelet_longitudinal_d;
-    result_lanelet_pose.offset = next_lanelet_pose.offset + lateral_d;
+    result_lanelet_pose.offset = lanelet_pose.offset;  // + lateral_d;
   } else {
     THROW_SIMULATION_ERROR(
       "Next lanelet is too short: lanelet_id==", next_lanelet_pose.lanelet_id, " is shorter than ",

From b2074d48c9e559cb37a558d2fcf0292f355bff8e Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Fri, 20 Dec 2024 16:10:37 +0100
Subject: [PATCH 24/38] fix(behavior_tree_plugin, traffic_simulator): fix
 moveTowardsLaneletPose for WalkStraightAction, tidy up

---
 .../behavior_tree_plugin/src/action_node.cpp  | 28 +++++--------------
 .../follow_polyline_trajectory_action.cpp     |  3 --
 .../src/behavior/follow_trajectory.cpp        | 11 +++-----
 .../traffic_simulator/src/utils/pose.cpp      | 15 +++-------
 4 files changed, 15 insertions(+), 42 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 2f6beac294b..09d3a588c5e 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -516,6 +516,8 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   using math::geometry::operator-;
   using math::geometry::operator+=;
 
+  constexpr bool desired_velocity_is_global{false};
+
   const auto include_crosswalk = [](const auto & entity_type) {
     return (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == entity_type.type) ||
            (traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == entity_type.type);
@@ -530,6 +532,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
       const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
     -> geometry_msgs::msg::Pose {
     geometry_msgs::msg::Pose updated_pose;
+
     // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s)
     geometry_msgs::msg::Vector3 delta_rotation;
     delta_rotation.z = desired_twist.angular.z * step_time;
@@ -547,39 +550,22 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
     const Eigen::Vector3d delta_position = rotation_matrix * translation;
     updated_pose.position = status->getMapPose().position + delta_position;
 
-    std::cout << " 1 " << std::endl;
     // if it is the transition between lanelet pose: overwrite position to improve precision
     if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
       const auto estimated_next_canonicalized_lanelet_pose =
         traffic_simulator::pose::toCanonicalizedLaneletPose(
           updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance,
           hdmap_utils_ptr);
-      const auto lanelet_pose =
-        static_cast<traffic_simulator::LaneletPose>(canonicalized_lanelet_pose.value());
-      const auto pose = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose.value());
-      std::cout << " 2 " << std::endl;
-      std::cout << " -- lanelet pose: id:" << lanelet_pose.lanelet_id << " s:" << lanelet_pose.s
-                << " offset:" << lanelet_pose.offset << " yaw: " << lanelet_pose.rpy.z << std::endl;
-      std::cout << " -- orientation: "
-                << math::geometry::convertQuaternionToEulerAngle(pose.orientation).z << std::endl;
-      std::cout << " -- d_velocity: x:" << desired_twist.linear.x << " y:" << desired_twist.linear.y
-                << " z:" << desired_twist.linear.z << std::endl;
+
       // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same
       if (estimated_next_canonicalized_lanelet_pose) {
         const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose(
           canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
-          desired_twist.linear, false, step_time, hdmap_utils_ptr);
-        std::cout << " 3 " << std::endl;
-        if (
-          const auto next_canonicalized_lanelet_pose =
-            traffic_simulator::pose::canonicalize(next_lanelet_pose, hdmap_utils_ptr)) {
-          std::cout << " 4 " << std::endl;
-          updated_pose.position =
-            static_cast<geometry_msgs::msg::Pose>(next_canonicalized_lanelet_pose.value()).position;
-        }
+          desired_twist.linear, desired_velocity_is_global, step_time, hdmap_utils_ptr);
+        updated_pose.position =
+          traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position;
       }
     }
-    std::cout << " ~~~~~ " << std::endl;
     return updated_pose;
   };
 
diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
index 9ae8d6e5889..07b48bae9c3 100644
--- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
+++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
@@ -63,7 +63,6 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
       return canonicalized_entity_status->getTwist().linear.x;
     }
   };
-  std::cout << "FollowPolylineTrajectoryAction" << std::endl;
   if (getBlackBoardValues();
       request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
@@ -83,10 +82,8 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
     setCanonicalizedEntityStatus(entity_status_updated.value());
     setOutput("waypoints", calculateWaypoints());
     setOutput("obstacle", calculateObstacle(calculateWaypoints()));
-    std::cout << "~~FollowPolylineTrajectoryAction" << std::endl;
     return BT::NodeStatus::RUNNING;
   } else {
-    std::cout << "~~FollowPolylineTrajectoryAction" << std::endl;
     return BT::NodeStatus::SUCCESS;
   }
 }
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 37807afb9d1..f0da884b3b5 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -579,6 +579,8 @@ auto makeUpdatedStatus(
 
     // if it is the transition between lanelet pose: overwrite position to improve precision
     if (entity_status.lanelet_pose_valid) {
+      constexpr bool desired_velocity_is_global{true};
+
       const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
         entity_status.pose, entity_status.bounding_box, {entity_status.lanelet_pose.lanelet_id},
         include_crosswalk, matching_distance, hdmap_utils);
@@ -591,13 +593,8 @@ auto makeUpdatedStatus(
       if (canonicalized_lanelet_pose && estimated_next_canonicalized_lanelet_pose) {
         const auto next_lanelet_pose = pose::moveTowardsLaneletPose(
           canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
-          desired_velocity, true, step_time, hdmap_utils);
-        if (
-          const auto next_canonicalized_lanelet_pose =
-            pose::canonicalize(next_lanelet_pose, hdmap_utils)) {
-          updated_status.pose.position =
-            static_cast<geometry_msgs::msg::Pose>(next_canonicalized_lanelet_pose.value()).position;
-        }
+          desired_velocity, desired_velocity_is_global, step_time, hdmap_utils);
+        updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position;
       }
     }
 
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 69e6bada0c0..65609776db9 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -155,17 +155,11 @@ auto moveTowardsLaneletPose(
   const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
   const auto next_lanelet_pose = static_cast<LaneletPose>(next_canonicalized_lanelet_pose);
 
-  // transform desired (global) velocity to local velocity
-  const auto orientation =
-    static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose).orientation;
-
-  // const auto fi = lanelet_pose.rpy.z;
-  // const auto orientation_rpy = math::geometry::convertQuaternionToEulerAngle(orientation);
-  // std::cout << "-- lanelet_pose.rpy.z: " << lanelet_pose.rpy.z
-  //           << " | orientation_rpy.z: " << orientation_rpy.z << " | fi: " << fi << std::endl;
-
   Eigen::Vector2d displacement;
   if (desired_velocity_is_global) {
+    // transform desired (global) velocity to local velocity
+    const auto orientation =
+      static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose).orientation;
     const Eigen::Vector3d global_velocity(
       desired_velocity.x, desired_velocity.y, desired_velocity.z);
     const Eigen::Quaterniond quaternion(orientation.w, orientation.x, orientation.y, orientation.z);
@@ -187,17 +181,16 @@ auto moveTowardsLaneletPose(
   if (longitudinal_d <= remaining_lanelet_length) {
     result_lanelet_pose.lanelet_id = lanelet_pose.lanelet_id;
     result_lanelet_pose.s = lanelet_pose.s + longitudinal_d;
-    result_lanelet_pose.offset = lanelet_pose.offset;  // + lateral_d;
   } else if (  // if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible
     next_lanelet_longitudinal_d < hdmap_utils_ptr->getLaneletLength(next_lanelet_pose.lanelet_id)) {
     result_lanelet_pose.lanelet_id = next_lanelet_pose.lanelet_id;
     result_lanelet_pose.s = next_lanelet_longitudinal_d;
-    result_lanelet_pose.offset = lanelet_pose.offset;  // + lateral_d;
   } else {
     THROW_SIMULATION_ERROR(
       "Next lanelet is too short: lanelet_id==", next_lanelet_pose.lanelet_id, " is shorter than ",
       next_lanelet_longitudinal_d);
   }
+  result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
   result_lanelet_pose.rpy = lanelet_pose.rpy;
   return result_lanelet_pose;
 }

From ad6c16b9d47f74506c644832f343856a3bd66d3c Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Fri, 20 Dec 2024 16:14:48 +0100
Subject: [PATCH 25/38] ref(traffic_simulator): tidy up after
 moveTowardsLaneletPose development

---
 .../follow_polyline_trajectory_action.cpp                     | 1 -
 .../traffic_simulator/src/behavior/follow_trajectory.cpp      | 4 ++--
 simulation/traffic_simulator/src/utils/pose.cpp               | 2 +-
 3 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
index 07b48bae9c3..d0f65ea7c80 100644
--- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
+++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
@@ -68,7 +68,6 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
       not getInput<decltype(target_speed)>("target_speed", target_speed) or
       not polyline_trajectory) {
-    std::cout << "~~FollowPolylineTrajectoryAction" << std::endl;
     return BT::NodeStatus::FAILURE;
   } else if (std::isnan(canonicalized_entity_status->getTime())) {
     THROW_SIMULATION_ERROR(
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index f0da884b3b5..ef9cbd8ea18 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -558,8 +558,6 @@ auto makeUpdatedStatus(
     */
     auto updated_status = entity_status;
 
-    updated_status.lanelet_pose_valid = false;
-
     updated_status.pose.position += desired_velocity * step_time;
 
     updated_status.pose.orientation = [&]() {
@@ -619,6 +617,8 @@ auto makeUpdatedStatus(
 
     updated_status.time = entity_status.time + step_time;
 
+    updated_status.lanelet_pose_valid = false;
+
     return updated_status;
   }
 }
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 65609776db9..a1b55266d3e 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -155,6 +155,7 @@ auto moveTowardsLaneletPose(
   const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
   const auto next_lanelet_pose = static_cast<LaneletPose>(next_canonicalized_lanelet_pose);
 
+  // determine the displacement in the 2D lanelet coordinate system
   Eigen::Vector2d displacement;
   if (desired_velocity_is_global) {
     // transform desired (global) velocity to local velocity
@@ -164,7 +165,6 @@ auto moveTowardsLaneletPose(
       desired_velocity.x, desired_velocity.y, desired_velocity.z);
     const Eigen::Quaterniond quaternion(orientation.w, orientation.x, orientation.y, orientation.z);
     const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity;
-    // determine the displacement in the 2D lanelet coordinate system
     displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
                    Eigen::Vector2d(local_velocity.x(), local_velocity.y()) * step_time;
   } else {

From 74bee395dd110f601708d07c86e43f12ab224aed Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Fri, 20 Dec 2024 16:19:49 +0100
Subject: [PATCH 26/38] fix(traffic_simulator): revert
 adjustOrientationAndOzPosition  change (lanelet rpy)

---
 simulation/traffic_simulator/src/data_type/lanelet_pose.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp
index 39b24b6c738..b4dfb4dc0d8 100644
--- a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp
+++ b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp
@@ -154,7 +154,7 @@ auto CanonicalizedLaneletPose::adjustOrientationAndOzPosition(
                                       .y(lanelet_rpy.y)
                                       .z(entity_rpy.z));
     lanelet_pose_.rpy =
-      convertQuaternionToEulerAngle(getRotation(map_pose_.orientation, lanelet_quaternion));
+      convertQuaternionToEulerAngle(getRotation(lanelet_quaternion, map_pose_.orientation));
   }
 }
 

From c3e604fa3587e8369d72c7c74a0fed59796eb0c2 Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Fri, 20 Dec 2024 18:23:56 +0100
Subject: [PATCH 27/38] fix(behavior_tree_plugin) fix sonarquebe issues

---
 .../behavior_tree_plugin/src/action_node.cpp  | 81 +++++++++----------
 1 file changed, 40 insertions(+), 41 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 09d3a588c5e..438f5b31620 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -528,46 +528,45 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   const auto buildUpdatedPose =
     [&include_crosswalk, &matching_distance](
       const std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> & status,
-      const geometry_msgs::msg::Twist & desired_twist, const double step_time,
-      const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
-    -> geometry_msgs::msg::Pose {
-    geometry_msgs::msg::Pose updated_pose;
-
-    // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s)
-    geometry_msgs::msg::Vector3 delta_rotation;
-    delta_rotation.z = desired_twist.angular.z * step_time;
-    const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
-    updated_pose.orientation = status->getMapPose().orientation * delta_quaternion;
-
-    // apply position change
-    /// @todo first determine global desired_velocity, calculate position change using it
-    // then pass the same global desired_velocity to moveTowardsLaneletPose()
-    const Eigen::Matrix3d rotation_matrix =
-      math::geometry::getRotationMatrix(updated_pose.orientation);
-    const Eigen::Vector3d translation = Eigen::Vector3d(
-      desired_twist.linear.x * step_time, desired_twist.linear.y * step_time,
-      desired_twist.linear.z * step_time);
-    const Eigen::Vector3d delta_position = rotation_matrix * translation;
-    updated_pose.position = status->getMapPose().position + delta_position;
-
-    // if it is the transition between lanelet pose: overwrite position to improve precision
-    if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
-      const auto estimated_next_canonicalized_lanelet_pose =
-        traffic_simulator::pose::toCanonicalizedLaneletPose(
-          updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance,
-          hdmap_utils_ptr);
-
-      // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same
-      if (estimated_next_canonicalized_lanelet_pose) {
-        const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose(
-          canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
-          desired_twist.linear, desired_velocity_is_global, step_time, hdmap_utils_ptr);
-        updated_pose.position =
-          traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position;
+      const geometry_msgs::msg::Twist & desired_twist, const double time_step,
+      const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) {
+      geometry_msgs::msg::Pose updated_pose;
+
+      // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s)
+      geometry_msgs::msg::Vector3 delta_rotation;
+      delta_rotation.z = desired_twist.angular.z * time_step;
+      const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
+      updated_pose.orientation = status->getMapPose().orientation * delta_quaternion;
+
+      // apply position change
+      /// @todo first determine global desired_velocity, calculate position change using it
+      // then pass the same global desired_velocity to moveTowardsLaneletPose()
+      const Eigen::Matrix3d rotation_matrix =
+        math::geometry::getRotationMatrix(updated_pose.orientation);
+      const auto translation = Eigen::Vector3d(
+        desired_twist.linear.x * time_step, desired_twist.linear.y * time_step,
+        desired_twist.linear.z * time_step);
+      const Eigen::Vector3d delta_position = rotation_matrix * translation;
+      updated_pose.position = status->getMapPose().position + delta_position;
+
+      // if it is the transition between lanelet pose: overwrite position to improve precision
+      if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
+        const auto estimated_next_canonicalized_lanelet_pose =
+          traffic_simulator::pose::toCanonicalizedLaneletPose(
+            updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance,
+            hdmap_utils_ptr);
+
+        // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same
+        if (estimated_next_canonicalized_lanelet_pose) {
+          const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose(
+            canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
+            desired_twist.linear, desired_velocity_is_global, time_step, hdmap_utils_ptr);
+          updated_pose.position =
+            traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position;
+        }
       }
-    }
-    return updated_pose;
-  };
+      return updated_pose;
+    };
 
   const auto speed_planner =
     traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner(
@@ -576,8 +575,8 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
     target_speed, constraints, canonicalized_entity_status->getTwist(),
     canonicalized_entity_status->getAccel());
   const auto linear_jerk_new = std::get<2>(dynamics);
-  const auto accel_new = std::get<1>(dynamics);
-  const auto twist_new = std::get<0>(dynamics);
+  const auto & accel_new = std::get<1>(dynamics);
+  const auto & twist_new = std::get<0>(dynamics);
   const auto pose_new =
     buildUpdatedPose(canonicalized_entity_status, twist_new, step_time, hdmap_utils);
 

From e884623c46134588545d8639a03df2bd75d31afe Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Thu, 23 Jan 2025 11:29:00 +0100
Subject: [PATCH 28/38] fix(traffic_simulator): Fix lanelet slope inaccuracies

 - Adjusted entity position when transitioning between lanelets to reduce slope errors
---
 .../behavior_tree_plugin/src/action_node.cpp  | 18 ++--
 .../include/traffic_simulator/utils/pose.hpp  | 11 ++-
 .../src/behavior/follow_trajectory.cpp        | 18 ++--
 .../traffic_simulator/src/utils/pose.cpp      | 83 ++++++++-----------
 4 files changed, 57 insertions(+), 73 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 438f5b31620..17e487539ff 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -540,7 +540,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
 
       // apply position change
       /// @todo first determine global desired_velocity, calculate position change using it
-      // then pass the same global desired_velocity to moveTowardsLaneletPose()
+      // then pass the same global desired_velocity to adjustPoseForLaneletTransition()
       const Eigen::Matrix3d rotation_matrix =
         math::geometry::getRotationMatrix(updated_pose.orientation);
       const auto translation = Eigen::Vector3d(
@@ -549,20 +549,22 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
       const Eigen::Vector3d delta_position = rotation_matrix * translation;
       updated_pose.position = status->getMapPose().position + delta_position;
 
-      // if it is the transition between lanelet pose: overwrite position to improve precision
       if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
         const auto estimated_next_canonicalized_lanelet_pose =
           traffic_simulator::pose::toCanonicalizedLaneletPose(
             updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance,
             hdmap_utils_ptr);
 
-        // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same
+        // if it is the transition between lanelets: overwrite position to improve precision
         if (estimated_next_canonicalized_lanelet_pose) {
-          const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose(
-            canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
-            desired_twist.linear, desired_velocity_is_global, time_step, hdmap_utils_ptr);
-          updated_pose.position =
-            traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position;
+          const auto next_lanelet_id = static_cast<traffic_simulator::LaneletPose>(
+                                         estimated_next_canonicalized_lanelet_pose.value())
+                                         .lanelet_id;
+          auto entity_status = static_cast<traffic_simulator::EntityStatus>(*status);
+          entity_status.pose = updated_pose;
+          updated_pose.position = traffic_simulator::pose::updatePositionForLaneletTransition(
+            entity_status, next_lanelet_id, desired_twist.linear, desired_velocity_is_global,
+            time_step, hdmap_utils_ptr);
         }
       }
       return updated_pose;
diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
index 93b13227fd2..07097b76c2d 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
@@ -60,12 +60,11 @@ auto transformRelativePoseToGlobal(
   const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose)
   -> geometry_msgs::msg::Pose;
 
-auto moveTowardsLaneletPose(
-  const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
-  const CanonicalizedLaneletPose & next_canonicalized_lanelet_pose,
-  const geometry_msgs::msg::Vector3 & desired_velocity, const bool desired_velocity_is_global,
-  const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
-  -> LaneletPose;
+auto updatePositionForLaneletTransition(
+  const traffic_simulator_msgs::msg::EntityStatus & entity_status,
+  const lanelet::Id next_lanelet_id, const geometry_msgs::msg::Vector3 & desired_velocity,
+  const bool desired_velocity_is_global, const double step_time,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Point;
 
 // Relative msg::Pose
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index ef9cbd8ea18..0d4e522704f 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -575,24 +575,20 @@ auto makeUpdatedStatus(
       }
     }();
 
-    // if it is the transition between lanelet pose: overwrite position to improve precision
     if (entity_status.lanelet_pose_valid) {
       constexpr bool desired_velocity_is_global{true};
-
-      const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
-        entity_status.pose, entity_status.bounding_box, {entity_status.lanelet_pose.lanelet_id},
-        include_crosswalk, matching_distance, hdmap_utils);
-
       const auto estimated_next_canonicalized_lanelet_pose =
         traffic_simulator::pose::toCanonicalizedLaneletPose(
           updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance,
           hdmap_utils);
 
-      if (canonicalized_lanelet_pose && estimated_next_canonicalized_lanelet_pose) {
-        const auto next_lanelet_pose = pose::moveTowardsLaneletPose(
-          canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
-          desired_velocity, desired_velocity_is_global, step_time, hdmap_utils);
-        updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position;
+      // if it is the transition between lanelets: overwrite position to improve precision
+      if (estimated_next_canonicalized_lanelet_pose) {
+        const auto next_lanelet_id =
+          static_cast<LaneletPose>(estimated_next_canonicalized_lanelet_pose.value()).lanelet_id;
+        updated_status.pose.position = pose::updatePositionForLaneletTransition(
+          updated_status, next_lanelet_id, desired_velocity, desired_velocity_is_global, step_time,
+          hdmap_utils);
       }
     }
 
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index dc5c353ac30..91bbd55f33c 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -142,61 +142,48 @@ auto transformRelativePoseToGlobal(
   return ret;
 }
 
-/// @note this function does not modify the orientation of LaneletPose
-// the orientation remains the same as in next_lanelet_pose
-auto moveTowardsLaneletPose(
-  const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
-  const CanonicalizedLaneletPose & next_canonicalized_lanelet_pose,
-  const geometry_msgs::msg::Vector3 & desired_velocity, const bool desired_velocity_is_global,
-  const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
-  -> LaneletPose
+/// @note this function does not modify the orientation of entity
+auto updatePositionForLaneletTransition(
+  const traffic_simulator_msgs::msg::EntityStatus & entity_status,
+  const lanelet::Id next_lanelet_id, const geometry_msgs::msg::Vector3 & desired_velocity,
+  const bool desired_velocity_is_global, const double step_time,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Point
 {
   using math::geometry::operator*;
-  using math::geometry::operator+;
-  using math::geometry::operator-;
   using math::geometry::operator+=;
 
-  const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
-  const auto next_lanelet_pose = static_cast<LaneletPose>(next_canonicalized_lanelet_pose);
-
-  // determine the displacement in the 2D lanelet coordinate system
-  Eigen::Vector2d displacement;
-  if (desired_velocity_is_global) {
-    // transform desired (global) velocity to local velocity
-    const auto orientation =
-      static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose).orientation;
-    const Eigen::Vector3d global_velocity(
-      desired_velocity.x, desired_velocity.y, desired_velocity.z);
-    const Eigen::Quaterniond quaternion(orientation.w, orientation.x, orientation.y, orientation.z);
-    const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity;
-    displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
-                   Eigen::Vector2d(local_velocity.x(), local_velocity.y()) * step_time;
-  } else {
-    displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
-                   Eigen::Vector2d(desired_velocity.x, desired_velocity.y) * step_time;
-  }
-  const auto longitudinal_d = displacement.x();
-  const auto lateral_d = displacement.y();
-
-  LaneletPose result_lanelet_pose;
+  const auto lanelet_pose = entity_status.lanelet_pose;
   const auto remaining_lanelet_length =
     hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s;
-  const auto next_lanelet_longitudinal_d = longitudinal_d - remaining_lanelet_length;
-  if (longitudinal_d <= remaining_lanelet_length) {
-    result_lanelet_pose.lanelet_id = lanelet_pose.lanelet_id;
-    result_lanelet_pose.s = lanelet_pose.s + longitudinal_d;
-  } else if (  // if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible
-    next_lanelet_longitudinal_d < hdmap_utils_ptr->getLaneletLength(next_lanelet_pose.lanelet_id)) {
-    result_lanelet_pose.lanelet_id = next_lanelet_pose.lanelet_id;
-    result_lanelet_pose.s = next_lanelet_longitudinal_d;
-  } else {
-    THROW_SIMULATION_ERROR(
-      "Next lanelet is too short: lanelet_id==", next_lanelet_pose.lanelet_id, " is shorter than ",
-      next_lanelet_longitudinal_d);
+  auto new_position = entity_status.pose.position;
+
+  /// Transition to the next lanelet
+  if (math::geometry::norm(desired_velocity * step_time) > remaining_lanelet_length) {
+    // determine the displacement in the 2D lanelet coordinate system
+    Eigen::Vector2d displacement;
+    if (desired_velocity_is_global) {
+      // transform desired (global) velocity to local velocity
+      const Eigen::Vector3d global_velocity(
+        desired_velocity.x, desired_velocity.y, desired_velocity.z);
+      const Eigen::Quaterniond quaternion(
+        entity_status.pose.orientation.w, entity_status.pose.orientation.x,
+        entity_status.pose.orientation.y, entity_status.pose.orientation.z);
+      const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity;
+      displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
+                     Eigen::Vector2d(local_velocity.x(), local_velocity.y()) * step_time;
+    } else {
+      displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
+                     Eigen::Vector2d(desired_velocity.x, desired_velocity.y) * step_time;
+    }
+
+    LaneletPose result_lanelet_pose;
+    result_lanelet_pose.lanelet_id = next_lanelet_id;
+    result_lanelet_pose.s = displacement.x() - remaining_lanelet_length;
+    result_lanelet_pose.offset = lanelet_pose.offset + displacement.y();
+    result_lanelet_pose.rpy = lanelet_pose.rpy;
+    new_position = toMapPose(result_lanelet_pose, hdmap_utils_ptr).position;
   }
-  result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
-  result_lanelet_pose.rpy = lanelet_pose.rpy;
-  return result_lanelet_pose;
+  return new_position;
 }
 
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)

From 2ba428fe1499da2d49320c3863d3a53d47ebf757 Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Thu, 23 Jan 2025 11:48:54 +0100
Subject: [PATCH 29/38] ref(traffic_simulator) Refactor
 updateEntityPositionForLaneletTransition

---
 simulation/behavior_tree_plugin/src/action_node.cpp    | 10 +++++-----
 .../include/traffic_simulator/utils/pose.hpp           |  2 +-
 .../src/behavior/follow_trajectory.cpp                 |  8 ++++----
 simulation/traffic_simulator/src/utils/pose.cpp        | 10 +++++-----
 4 files changed, 15 insertions(+), 15 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 17e487539ff..6622e12889f 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -532,15 +532,15 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
       const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) {
       geometry_msgs::msg::Pose updated_pose;
 
-      // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s)
+      // Apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s).
       geometry_msgs::msg::Vector3 delta_rotation;
       delta_rotation.z = desired_twist.angular.z * time_step;
       const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
       updated_pose.orientation = status->getMapPose().orientation * delta_quaternion;
 
-      // apply position change
+      // Apply position change.
       /// @todo first determine global desired_velocity, calculate position change using it
-      // then pass the same global desired_velocity to adjustPoseForLaneletTransition()
+      // then pass the same global desired_velocity to adjustPoseForLaneletTransition().
       const Eigen::Matrix3d rotation_matrix =
         math::geometry::getRotationMatrix(updated_pose.orientation);
       const auto translation = Eigen::Vector3d(
@@ -555,14 +555,14 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
             updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance,
             hdmap_utils_ptr);
 
-        // if it is the transition between lanelets: overwrite position to improve precision
+        // If it is the transition between lanelets: overwrite position to improve precision.
         if (estimated_next_canonicalized_lanelet_pose) {
           const auto next_lanelet_id = static_cast<traffic_simulator::LaneletPose>(
                                          estimated_next_canonicalized_lanelet_pose.value())
                                          .lanelet_id;
           auto entity_status = static_cast<traffic_simulator::EntityStatus>(*status);
           entity_status.pose = updated_pose;
-          updated_pose.position = traffic_simulator::pose::updatePositionForLaneletTransition(
+          updated_pose.position = traffic_simulator::pose::updateEntityPositionForLaneletTransition(
             entity_status, next_lanelet_id, desired_twist.linear, desired_velocity_is_global,
             time_step, hdmap_utils_ptr);
         }
diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
index 07097b76c2d..380faf5fb92 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
@@ -60,7 +60,7 @@ auto transformRelativePoseToGlobal(
   const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose)
   -> geometry_msgs::msg::Pose;
 
-auto updatePositionForLaneletTransition(
+auto updateEntityPositionForLaneletTransition(
   const traffic_simulator_msgs::msg::EntityStatus & entity_status,
   const lanelet::Id next_lanelet_id, const geometry_msgs::msg::Vector3 & desired_velocity,
   const bool desired_velocity_is_global, const double step_time,
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 0d4e522704f..78a2c6f1b3b 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -562,10 +562,10 @@ auto makeUpdatedStatus(
 
     updated_status.pose.orientation = [&]() {
       if (desired_velocity.y == 0 && desired_velocity.x == 0 && desired_velocity.z == 0) {
-        // do not change orientation if there is no designed_velocity vector
+        // Do not change orientation if there is no designed_velocity vector.
         return entity_status.pose.orientation;
       } else {
-        // if there is a designed_velocity vector, set the orientation in the direction of it
+        // If there is a designed_velocity vector, set the orientation in the direction of it.
         const geometry_msgs::msg::Vector3 direction =
           geometry_msgs::build<geometry_msgs::msg::Vector3>()
             .x(0.0)
@@ -582,11 +582,11 @@ auto makeUpdatedStatus(
           updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance,
           hdmap_utils);
 
-      // if it is the transition between lanelets: overwrite position to improve precision
+      // If it is the transition between lanelets: overwrite position to improve precision.
       if (estimated_next_canonicalized_lanelet_pose) {
         const auto next_lanelet_id =
           static_cast<LaneletPose>(estimated_next_canonicalized_lanelet_pose.value()).lanelet_id;
-        updated_status.pose.position = pose::updatePositionForLaneletTransition(
+        updated_status.pose.position = pose::updateEntityPositionForLaneletTransition(
           updated_status, next_lanelet_id, desired_velocity, desired_velocity_is_global, step_time,
           hdmap_utils);
       }
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 91bbd55f33c..1eccfe3a710 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -142,8 +142,8 @@ auto transformRelativePoseToGlobal(
   return ret;
 }
 
-/// @note this function does not modify the orientation of entity
-auto updatePositionForLaneletTransition(
+/// @note This function does not modify the orientation of entity.
+auto updateEntityPositionForLaneletTransition(
   const traffic_simulator_msgs::msg::EntityStatus & entity_status,
   const lanelet::Id next_lanelet_id, const geometry_msgs::msg::Vector3 & desired_velocity,
   const bool desired_velocity_is_global, const double step_time,
@@ -157,12 +157,12 @@ auto updatePositionForLaneletTransition(
     hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s;
   auto new_position = entity_status.pose.position;
 
-  /// Transition to the next lanelet
+  // Transition to the next lanelet if the entity's displacement exceeds the remaining length
   if (math::geometry::norm(desired_velocity * step_time) > remaining_lanelet_length) {
-    // determine the displacement in the 2D lanelet coordinate system
+    // Determine the displacement in the 2D lanelet coordinate system
     Eigen::Vector2d displacement;
     if (desired_velocity_is_global) {
-      // transform desired (global) velocity to local velocity
+      // Transform desired (global) velocity to local velocity
       const Eigen::Vector3d global_velocity(
         desired_velocity.x, desired_velocity.y, desired_velocity.z);
       const Eigen::Quaterniond quaternion(

From c6733e3461671380e06d6193b6fe9501afcfd561 Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Fri, 24 Jan 2025 16:11:49 +0100
Subject: [PATCH 30/38] fix(traffic_simulator) Improve lanelet transition
 handling for entity position updates

---
 .../behavior_tree_plugin/src/action_node.cpp  | 23 +++----
 .../include/traffic_simulator/utils/pose.hpp  |  5 +-
 .../src/behavior/follow_trajectory.cpp        | 16 ++---
 .../traffic_simulator/src/utils/pose.cpp      | 61 +++++++++++--------
 4 files changed, 58 insertions(+), 47 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 6622e12889f..83374ca08e6 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -513,7 +513,6 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
 {
   using math::geometry::operator*;
   using math::geometry::operator+;
-  using math::geometry::operator-;
   using math::geometry::operator+=;
 
   constexpr bool desired_velocity_is_global{false};
@@ -532,15 +531,15 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
       const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) {
       geometry_msgs::msg::Pose updated_pose;
 
-      // Apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s).
+      // Apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s)
       geometry_msgs::msg::Vector3 delta_rotation;
       delta_rotation.z = desired_twist.angular.z * time_step;
       const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
       updated_pose.orientation = status->getMapPose().orientation * delta_quaternion;
 
-      // Apply position change.
+      // Apply position change
       /// @todo first determine global desired_velocity, calculate position change using it
-      // then pass the same global desired_velocity to adjustPoseForLaneletTransition().
+      // then pass the same global desired_velocity to adjustPoseForLaneletTransition()
       const Eigen::Matrix3d rotation_matrix =
         math::geometry::getRotationMatrix(updated_pose.orientation);
       const auto translation = Eigen::Vector3d(
@@ -549,22 +548,24 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
       const Eigen::Vector3d delta_position = rotation_matrix * translation;
       updated_pose.position = status->getMapPose().position + delta_position;
 
+      // If it is the transition between lanelets: overwrite position to improve precision
       if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
         const auto estimated_next_canonicalized_lanelet_pose =
           traffic_simulator::pose::toCanonicalizedLaneletPose(
             updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance,
             hdmap_utils_ptr);
-
-        // If it is the transition between lanelets: overwrite position to improve precision.
         if (estimated_next_canonicalized_lanelet_pose) {
+          const auto entity_status = static_cast<traffic_simulator::EntityStatus>(*status);
           const auto next_lanelet_id = static_cast<traffic_simulator::LaneletPose>(
                                          estimated_next_canonicalized_lanelet_pose.value())
                                          .lanelet_id;
-          auto entity_status = static_cast<traffic_simulator::EntityStatus>(*status);
-          entity_status.pose = updated_pose;
-          updated_pose.position = traffic_simulator::pose::updateEntityPositionForLaneletTransition(
-            entity_status, next_lanelet_id, desired_twist.linear, desired_velocity_is_global,
-            time_step, hdmap_utils_ptr);
+          if (  // Handle lanelet transition
+            const auto updated_position =
+              traffic_simulator::pose::updatePositionForLaneletTransition(
+                entity_status, next_lanelet_id, desired_twist.linear, desired_velocity_is_global,
+                time_step, hdmap_utils_ptr)) {
+            updated_pose.position = updated_position.value();
+          }
         }
       }
       return updated_pose;
diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
index 380faf5fb92..369d81eaac2 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
@@ -60,11 +60,12 @@ auto transformRelativePoseToGlobal(
   const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose)
   -> geometry_msgs::msg::Pose;
 
-auto updateEntityPositionForLaneletTransition(
+auto updatePositionForLaneletTransition(
   const traffic_simulator_msgs::msg::EntityStatus & entity_status,
   const lanelet::Id next_lanelet_id, const geometry_msgs::msg::Vector3 & desired_velocity,
   const bool desired_velocity_is_global, const double step_time,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Point;
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+  -> std::optional<geometry_msgs::msg::Point>;
 
 // Relative msg::Pose
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 78a2c6f1b3b..5f6341afa6e 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -562,10 +562,10 @@ auto makeUpdatedStatus(
 
     updated_status.pose.orientation = [&]() {
       if (desired_velocity.y == 0 && desired_velocity.x == 0 && desired_velocity.z == 0) {
-        // Do not change orientation if there is no designed_velocity vector.
+        // Do not change orientation if there is no designed_velocity vector
         return entity_status.pose.orientation;
       } else {
-        // If there is a designed_velocity vector, set the orientation in the direction of it.
+        // If there is a designed_velocity vector, set the orientation in the direction of it
         const geometry_msgs::msg::Vector3 direction =
           geometry_msgs::build<geometry_msgs::msg::Vector3>()
             .x(0.0)
@@ -575,20 +575,22 @@ auto makeUpdatedStatus(
       }
     }();
 
+    // If it is the transition between lanelets: overwrite position to improve precision
     if (entity_status.lanelet_pose_valid) {
       constexpr bool desired_velocity_is_global{true};
       const auto estimated_next_canonicalized_lanelet_pose =
         traffic_simulator::pose::toCanonicalizedLaneletPose(
           updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance,
           hdmap_utils);
-
-      // If it is the transition between lanelets: overwrite position to improve precision.
       if (estimated_next_canonicalized_lanelet_pose) {
         const auto next_lanelet_id =
           static_cast<LaneletPose>(estimated_next_canonicalized_lanelet_pose.value()).lanelet_id;
-        updated_status.pose.position = pose::updateEntityPositionForLaneletTransition(
-          updated_status, next_lanelet_id, desired_velocity, desired_velocity_is_global, step_time,
-          hdmap_utils);
+        if (  // Handle lanelet transition
+          const auto updated_position = pose::updatePositionForLaneletTransition(
+            entity_status, next_lanelet_id, desired_velocity, desired_velocity_is_global, step_time,
+            hdmap_utils)) {
+          updated_status.pose.position = updated_position.value();
+        }
       }
     }
 
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 1eccfe3a710..e8ea7673586 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -143,47 +143,54 @@ auto transformRelativePoseToGlobal(
 }
 
 /// @note This function does not modify the orientation of entity.
-auto updateEntityPositionForLaneletTransition(
+auto updatePositionForLaneletTransition(
   const traffic_simulator_msgs::msg::EntityStatus & entity_status,
   const lanelet::Id next_lanelet_id, const geometry_msgs::msg::Vector3 & desired_velocity,
   const bool desired_velocity_is_global, const double step_time,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Point
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+  -> std::optional<geometry_msgs::msg::Point>
 {
   using math::geometry::operator*;
   using math::geometry::operator+=;
 
   const auto lanelet_pose = entity_status.lanelet_pose;
+  // Determine the displacement in the 2D lanelet coordinate system
+  Eigen::Vector2d displacement;
+  if (desired_velocity_is_global) {
+    // Transform desired (global) velocity to local velocity
+    const Eigen::Vector3d global_velocity(
+      desired_velocity.x, desired_velocity.y, desired_velocity.z);
+    const Eigen::Quaterniond quaternion(
+      entity_status.pose.orientation.w, entity_status.pose.orientation.x,
+      entity_status.pose.orientation.y, entity_status.pose.orientation.z);
+    const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity;
+    displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
+                   Eigen::Vector2d(local_velocity.x(), local_velocity.y()) * step_time;
+  } else {
+    displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
+                   Eigen::Vector2d(desired_velocity.x, desired_velocity.y) * step_time;
+  }
+  const auto longitudinal_d = displacement.x();
+  const auto lateral_d = displacement.y();
+
   const auto remaining_lanelet_length =
     hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s;
-  auto new_position = entity_status.pose.position;
-
-  // Transition to the next lanelet if the entity's displacement exceeds the remaining length
-  if (math::geometry::norm(desired_velocity * step_time) > remaining_lanelet_length) {
-    // Determine the displacement in the 2D lanelet coordinate system
-    Eigen::Vector2d displacement;
-    if (desired_velocity_is_global) {
-      // Transform desired (global) velocity to local velocity
-      const Eigen::Vector3d global_velocity(
-        desired_velocity.x, desired_velocity.y, desired_velocity.z);
-      const Eigen::Quaterniond quaternion(
-        entity_status.pose.orientation.w, entity_status.pose.orientation.x,
-        entity_status.pose.orientation.y, entity_status.pose.orientation.z);
-      const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity;
-      displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
-                     Eigen::Vector2d(local_velocity.x(), local_velocity.y()) * step_time;
-    } else {
-      displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
-                     Eigen::Vector2d(desired_velocity.x, desired_velocity.y) * step_time;
-    }
-
+  const auto next_lanelet_longitudinal_d = longitudinal_d - remaining_lanelet_length;
+  if (longitudinal_d <= remaining_lanelet_length) {
+    return std::nullopt;
+  } else if (  // if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible
+    next_lanelet_longitudinal_d < hdmap_utils_ptr->getLaneletLength(next_lanelet_id)) {
     LaneletPose result_lanelet_pose;
     result_lanelet_pose.lanelet_id = next_lanelet_id;
-    result_lanelet_pose.s = displacement.x() - remaining_lanelet_length;
-    result_lanelet_pose.offset = lanelet_pose.offset + displacement.y();
+    result_lanelet_pose.s = next_lanelet_longitudinal_d;
+    result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
     result_lanelet_pose.rpy = lanelet_pose.rpy;
-    new_position = toMapPose(result_lanelet_pose, hdmap_utils_ptr).position;
+    return toMapPose(result_lanelet_pose, hdmap_utils_ptr).position;
+  } else {
+    THROW_SIMULATION_ERROR(
+      "Next lanelet is too short: lanelet_id==", next_lanelet_id, " is shorter than ",
+      next_lanelet_longitudinal_d);
   }
-  return new_position;
 }
 
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)

From 404f4288e2a1b940b21b9bfa8d2923530ef1b40b Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Mon, 27 Jan 2025 11:33:04 +0100
Subject: [PATCH 31/38] ref(traffic_simulator> Refactor code according to the
 lastest master branch changes

---
 .../behavior_tree_plugin/src/action_node.cpp  | 18 ++++++--------
 .../follow_polyline_trajectory_action.cpp     |  1 +
 .../include/traffic_simulator/utils/pose.hpp  |  8 +++----
 .../src/behavior/follow_trajectory.cpp        | 11 +++++----
 .../traffic_simulator/src/utils/pose.cpp      | 24 +++++++++----------
 5 files changed, 29 insertions(+), 33 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index c7980bc2063..b8c01c1d774 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -479,19 +479,18 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   const auto buildUpdatedPose =
     [&include_crosswalk, &matching_distance](
       const std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> & status,
-      const geometry_msgs::msg::Twist & desired_twist, const double time_step,
-      const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) {
+      const geometry_msgs::msg::Twist & desired_twist, const double time_step) {
       geometry_msgs::msg::Pose updated_pose;
 
       // Apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s)
       geometry_msgs::msg::Vector3 delta_rotation;
-      delta_rotation.z = desired_twist.angular.z * time_step;
+      delta_rotation = desired_twist.angular * time_step;
       const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
       updated_pose.orientation = status->getMapPose().orientation * delta_quaternion;
 
       // Apply position change
       /// @todo first determine global desired_velocity, calculate position change using it
-      // then pass the same global desired_velocity to adjustPoseForLaneletTransition()
+      /// then pass the same global desired_velocity to adjustPoseForLaneletTransition()
       const Eigen::Matrix3d rotation_matrix =
         math::geometry::getRotationMatrix(updated_pose.orientation);
       const auto translation = Eigen::Vector3d(
@@ -504,18 +503,16 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
       if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
         const auto estimated_next_canonicalized_lanelet_pose =
           traffic_simulator::pose::toCanonicalizedLaneletPose(
-            updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance,
-            hdmap_utils_ptr);
+            updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance);
         if (estimated_next_canonicalized_lanelet_pose) {
-          const auto entity_status = static_cast<traffic_simulator::EntityStatus>(*status);
           const auto next_lanelet_id = static_cast<traffic_simulator::LaneletPose>(
                                          estimated_next_canonicalized_lanelet_pose.value())
                                          .lanelet_id;
           if (  // Handle lanelet transition
             const auto updated_position =
               traffic_simulator::pose::updatePositionForLaneletTransition(
-                entity_status, next_lanelet_id, desired_twist.linear, desired_velocity_is_global,
-                time_step, hdmap_utils_ptr)) {
+                canonicalized_lanelet_pose.value(), next_lanelet_id, desired_twist.linear,
+                desired_velocity_is_global, time_step)) {
             updated_pose.position = updated_position.value();
           }
         }
@@ -532,8 +529,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   const auto linear_jerk_new = std::get<2>(dynamics);
   const auto & accel_new = std::get<1>(dynamics);
   const auto & twist_new = std::get<0>(dynamics);
-  const auto pose_new =
-    buildUpdatedPose(canonicalized_entity_status, twist_new, step_time, hdmap_utils);
+  const auto pose_new = buildUpdatedPose(canonicalized_entity_status, twist_new, step_time);
 
   auto entity_status_updated =
     static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status);
diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
index d0f65ea7c80..8e722f5afb0 100644
--- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
+++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
@@ -63,6 +63,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
       return canonicalized_entity_status->getTwist().linear.x;
     }
   };
+
   if (getBlackBoardValues();
       request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
index 764dce4f483..352ed94c9eb 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
@@ -61,11 +61,9 @@ auto transformRelativePoseToGlobal(
   -> geometry_msgs::msg::Pose;
 
 auto updatePositionForLaneletTransition(
-  const traffic_simulator_msgs::msg::EntityStatus & entity_status,
-  const lanelet::Id next_lanelet_id, const geometry_msgs::msg::Vector3 & desired_velocity,
-  const bool desired_velocity_is_global, const double step_time,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
-  -> std::optional<geometry_msgs::msg::Point>;
+  const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id next_lanelet_id,
+  const geometry_msgs::msg::Vector3 & desired_velocity, const bool desired_velocity_is_global,
+  const double step_time) -> std::optional<geometry_msgs::msg::Point>;
 
 // Relative msg::Pose
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 0f2c0af28c9..39c687aff43 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -583,17 +583,18 @@ auto makeUpdatedStatus(
     // If it is the transition between lanelets: overwrite position to improve precision
     if (entity_status.lanelet_pose_valid) {
       constexpr bool desired_velocity_is_global{true};
+      const auto canonicalized_lanelet_pose = traffic_simulator::pose::toCanonicalizedLaneletPose(
+        entity_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance);
       const auto estimated_next_canonicalized_lanelet_pose =
         traffic_simulator::pose::toCanonicalizedLaneletPose(
-          updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance,
-          hdmap_utils);
-      if (estimated_next_canonicalized_lanelet_pose) {
+          updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance);
+      if (canonicalized_lanelet_pose && estimated_next_canonicalized_lanelet_pose) {
         const auto next_lanelet_id =
           static_cast<LaneletPose>(estimated_next_canonicalized_lanelet_pose.value()).lanelet_id;
         if (  // Handle lanelet transition
           const auto updated_position = pose::updatePositionForLaneletTransition(
-            entity_status, next_lanelet_id, desired_velocity, desired_velocity_is_global, step_time,
-            hdmap_utils)) {
+            canonicalized_lanelet_pose.value(), next_lanelet_id, desired_velocity,
+            desired_velocity_is_global, step_time)) {
           updated_status.pose.position = updated_position.value();
         }
       }
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 4be4a539d97..c0424c2b015 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -179,16 +179,16 @@ auto transformRelativePoseToGlobal(
 
 /// @note This function does not modify the orientation of entity.
 auto updatePositionForLaneletTransition(
-  const traffic_simulator_msgs::msg::EntityStatus & entity_status,
-  const lanelet::Id next_lanelet_id, const geometry_msgs::msg::Vector3 & desired_velocity,
-  const bool desired_velocity_is_global, const double step_time,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
-  -> std::optional<geometry_msgs::msg::Point>
+  const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id next_lanelet_id,
+  const geometry_msgs::msg::Vector3 & desired_velocity, const bool desired_velocity_is_global,
+  const double step_time) -> std::optional<geometry_msgs::msg::Point>
 {
   using math::geometry::operator*;
   using math::geometry::operator+=;
 
-  const auto lanelet_pose = entity_status.lanelet_pose;
+  const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
+  const auto map_pose = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose);
+
   // Determine the displacement in the 2D lanelet coordinate system
   Eigen::Vector2d displacement;
   if (desired_velocity_is_global) {
@@ -196,8 +196,8 @@ auto updatePositionForLaneletTransition(
     const Eigen::Vector3d global_velocity(
       desired_velocity.x, desired_velocity.y, desired_velocity.z);
     const Eigen::Quaterniond quaternion(
-      entity_status.pose.orientation.w, entity_status.pose.orientation.x,
-      entity_status.pose.orientation.y, entity_status.pose.orientation.z);
+      map_pose.orientation.w, map_pose.orientation.x, map_pose.orientation.y,
+      map_pose.orientation.z);
     const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity;
     displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
                    Eigen::Vector2d(local_velocity.x(), local_velocity.y()) * step_time;
@@ -209,25 +209,25 @@ auto updatePositionForLaneletTransition(
   const auto lateral_d = displacement.y();
 
   const auto remaining_lanelet_length =
-    hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s;
+    lanelet_wrapper::lanelet_map::laneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s;
   const auto next_lanelet_longitudinal_d = longitudinal_d - remaining_lanelet_length;
   if (longitudinal_d <= remaining_lanelet_length) {
     return std::nullopt;
   } else if (  // if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible
-    next_lanelet_longitudinal_d < hdmap_utils_ptr->getLaneletLength(next_lanelet_id)) {
+    next_lanelet_longitudinal_d < lanelet_wrapper::lanelet_map::laneletLength(next_lanelet_id)) {
     LaneletPose result_lanelet_pose;
     result_lanelet_pose.lanelet_id = next_lanelet_id;
     result_lanelet_pose.s = next_lanelet_longitudinal_d;
     result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
     result_lanelet_pose.rpy = lanelet_pose.rpy;
-    return toMapPose(result_lanelet_pose, hdmap_utils_ptr).position;
+    return toMapPose(result_lanelet_pose).position;
   } else {
     THROW_SIMULATION_ERROR(
       "Next lanelet is too short: lanelet_id==", next_lanelet_id, " is shorter than ",
       next_lanelet_longitudinal_d);
   }
 }
- 
+
 /// @note Relative msg::Pose
 auto isAltitudeMatching(
   const CanonicalizedLaneletPose & lanelet_pose,

From 44870e38d0f45345593d86fa622ac459b29b7be4 Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Mon, 27 Jan 2025 11:36:02 +0100
Subject: [PATCH 32/38] reref(behavior_tree_plugin) Update commment

---
 simulation/behavior_tree_plugin/src/action_node.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index b8c01c1d774..3d7d021e296 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -490,7 +490,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
 
       // Apply position change
       /// @todo first determine global desired_velocity, calculate position change using it
-      /// then pass the same global desired_velocity to adjustPoseForLaneletTransition()
+      /// then pass the same global desired_velocity to updatePositionForLaneletTransition()
       const Eigen::Matrix3d rotation_matrix =
         math::geometry::getRotationMatrix(updated_pose.orientation);
       const auto translation = Eigen::Vector3d(

From d81796b3671a76a296fc841e998085ce48b17899 Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Mon, 27 Jan 2025 11:58:02 +0100
Subject: [PATCH 33/38] ref(traffic_simulator): Refactor comments

---
 simulation/behavior_tree_plugin/src/action_node.cpp | 8 +++-----
 simulation/traffic_simulator/src/utils/pose.cpp     | 8 +++-----
 2 files changed, 6 insertions(+), 10 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 3d7d021e296..6414b941c09 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -254,9 +254,7 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf
     const auto quat = math::geometry::getRotation(
       canonicalized_entity_status->getMapPose().orientation,
       other_entity_status.at(each.first).getMapPose().orientation);
-    /**
-     * @note hard-coded parameter, if the Yaw value of RPY is in ~1.5708 -> 1.5708, entity is a candidate of front entity.
-     */
+    /// @note hard-coded parameter, if the Yaw value of RPY is in ~1.5708 -> 1.5708, entity is a candidate of front entity.
     if (
       std::fabs(math::geometry::convertQuaternionToEulerAngle(quat).z) <=
       boost::math::constants::half_pi<double>()) {
@@ -489,8 +487,8 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
       updated_pose.orientation = status->getMapPose().orientation * delta_quaternion;
 
       // Apply position change
-      /// @todo first determine global desired_velocity, calculate position change using it
-      /// then pass the same global desired_velocity to updatePositionForLaneletTransition()
+      // @todo first determine global desired_velocity, calculate position change using it
+      // then pass the same global desired_velocity to updatePositionForLaneletTransition()
       const Eigen::Matrix3d rotation_matrix =
         math::geometry::getRotationMatrix(updated_pose.orientation);
       const auto translation = Eigen::Vector3d(
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index c0424c2b015..c4bae8cb08d 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -187,12 +187,12 @@ auto updatePositionForLaneletTransition(
   using math::geometry::operator+=;
 
   const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
-  const auto map_pose = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose);
 
   // Determine the displacement in the 2D lanelet coordinate system
   Eigen::Vector2d displacement;
   if (desired_velocity_is_global) {
     // Transform desired (global) velocity to local velocity
+    const auto map_pose = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose);
     const Eigen::Vector3d global_velocity(
       desired_velocity.x, desired_velocity.y, desired_velocity.z);
     const Eigen::Quaterniond quaternion(
@@ -402,10 +402,8 @@ auto transformToCanonicalizedLaneletPose(
       map_pose, bounding_box, unique_route_lanelets, include_crosswalk, matching_distance)) {
     return canonicalized_lanelet_pose;
   }
-  /**
-   * @note Hard coded parameter. 2.0 is a matching threshold for lanelet.
-   * In this branch, the algorithm only consider entity pose.
-   */
+  /// @note Hard coded parameter. 2.0 is a matching threshold for lanelet.
+  /// In this branch, the algorithm only consider entity pose.
   if (
     const auto lanelet_pose =
       lanelet_wrapper::pose::toLaneletPose(map_pose, include_crosswalk, 2.0)) {

From 79e78187e94d521ad61c6cbc97477c48864a9e62 Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Mon, 27 Jan 2025 12:27:48 +0100
Subject: [PATCH 34/38] ref(traffic_simulator): Refactor comments

---
 .../behavior_tree_plugin/src/action_node.cpp  |  14 +-
 .../src/behavior/follow_trajectory.cpp        | 244 ++++++++----------
 .../traffic_simulator/src/utils/pose.cpp      |  22 +-
 3 files changed, 123 insertions(+), 157 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 6414b941c09..f2d7211558c 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -254,7 +254,7 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf
     const auto quat = math::geometry::getRotation(
       canonicalized_entity_status->getMapPose().orientation,
       other_entity_status.at(each.first).getMapPose().orientation);
-    /// @note hard-coded parameter, if the Yaw value of RPY is in ~1.5708 -> 1.5708, entity is a candidate of front entity.
+    /// @note hard-coded parameter, if the Yaw value of RPY is in [-pi/2, pi/2], entity is a candidate of front entity.
     if (
       std::fabs(math::geometry::convertQuaternionToEulerAngle(quat).z) <=
       boost::math::constants::half_pi<double>()) {
@@ -480,15 +480,15 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
       const geometry_msgs::msg::Twist & desired_twist, const double time_step) {
       geometry_msgs::msg::Pose updated_pose;
 
-      // Apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s)
+      /// @note Apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s)
       geometry_msgs::msg::Vector3 delta_rotation;
       delta_rotation = desired_twist.angular * time_step;
       const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
       updated_pose.orientation = status->getMapPose().orientation * delta_quaternion;
 
-      // Apply position change
-      // @todo first determine global desired_velocity, calculate position change using it
-      // then pass the same global desired_velocity to updatePositionForLaneletTransition()
+      /// @note Apply position change
+      /// @todo first determine global desired_velocity, calculate position change using it
+      /// then pass the same global desired_velocity to updatePositionForLaneletTransition()
       const Eigen::Matrix3d rotation_matrix =
         math::geometry::getRotationMatrix(updated_pose.orientation);
       const auto translation = Eigen::Vector3d(
@@ -497,7 +497,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
       const Eigen::Vector3d delta_position = rotation_matrix * translation;
       updated_pose.position = status->getMapPose().position + delta_position;
 
-      // If it is the transition between lanelets: overwrite position to improve precision
+      /// @note If it is the transition between lanelets: overwrite position to improve precision
       if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
         const auto estimated_next_canonicalized_lanelet_pose =
           traffic_simulator::pose::toCanonicalizedLaneletPose(
@@ -506,7 +506,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
           const auto next_lanelet_id = static_cast<traffic_simulator::LaneletPose>(
                                          estimated_next_canonicalized_lanelet_pose.value())
                                          .lanelet_id;
-          if (  // Handle lanelet transition
+          if (  /// @note Handle lanelet transition
             const auto updated_position =
               traffic_simulator::pose::updatePositionForLaneletTransition(
                 canonicalized_lanelet_pose.value(), next_lanelet_id, desired_twist.linear,
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 39c687aff43..eac06de886f 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -101,23 +101,21 @@ auto makeUpdatedStatus(
   };
 
   auto discard_the_front_waypoint_and_recurse = [&]() {
-    /*
-       The OpenSCENARIO standard does not define the behavior when the value of
-       Timing.domainAbsoluteRelative is "relative". The standard only states
-       "Definition of time value context as either absolute or relative", and
-       it is completely unclear when the relative time starts.
-
-       This implementation has interpreted the specification as follows:
-       Relative time starts from the start of FollowTrajectoryAction or from
-       the time of reaching the previous "waypoint with arrival time".
-
-       Note: not std::isnan(polyline_trajectory.base_time) means
-       "Timing.domainAbsoluteRelative is relative".
-
-       Note: not std::isnan(polyline_trajectory.shape.vertices.front().time)
-       means "The waypoint about to be popped is the waypoint with the
-       specified arrival time".
-    */
+    /// @note The OpenSCENARIO standard does not define the behavior when the value of
+    /// Timing.domainAbsoluteRelative is "relative". The standard only states
+    /// "Definition of time value context as either absolute or relative", and
+    /// it is completely unclear when the relative time starts.
+    ///
+    /// This implementation has interpreted the specification as follows:
+    /// Relative time starts from the start of FollowTrajectoryAction or from
+    /// the time of reaching the previous "waypoint with arrival time".
+    ///
+    /// Note: not std::isnan(polyline_trajectory.base_time) means
+    /// "Timing.domainAbsoluteRelative is relative".
+    ///
+    /// Note: not std::isnan(polyline_trajectory.shape.vertices.front().time)
+    /// means "The waypoint about to be popped is the waypoint with the
+    /// specified arrival time".
     if (
       not std::isnan(polyline_trajectory.base_time) and
       not std::isnan(polyline_trajectory.shape.vertices.front().time)) {
@@ -150,13 +148,11 @@ auto makeUpdatedStatus(
            std::prev(polyline_trajectory.shape.vertices.end());
   };
 
-  /*
-     The following code implements the steering behavior known as "seek". See
-     "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more
-     information.
-
-     See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters
-  */
+  /// @note The following code implements the steering behavior known as "seek". See
+  /// "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more
+  /// information.
+  ///
+  /// See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters
   if (polyline_trajectory.shape.vertices.empty()) {
     return std::nullopt;
   } else if (const auto position = entity_status.pose.position; any(is_infinity_or_nan, position)) {
@@ -166,11 +162,9 @@ auto makeUpdatedStatus(
       std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [",
       position.x, ", ", position.y, ", ", position.z, "].");
   } else if (
-    /*
-       We've made sure that polyline_trajectory.shape.vertices is not empty, so
-       a reference to vertices.front() always succeeds. vertices.front() is
-       referenced only this once in this member function.
-    */
+    /// @note We've made sure that polyline_trajectory.shape.vertices is not empty, so
+    /// a reference to vertices.front() always succeeds. vertices.front() is
+    /// referenced only this once in this member function.
     const auto target_position = polyline_trajectory.shape.vertices.front().position.position;
     any(is_infinity_or_nan, target_position)) {
     throw common::Error(
@@ -180,30 +174,24 @@ auto makeUpdatedStatus(
       "'s target position coordinate value contains NaN or infinity. The value is [",
       target_position.x, ", ", target_position.y, ", ", target_position.z, "].");
   } else if (
-    /*
-       If not dynamic_constraints_ignorable, the linear distance should cause
-       problems.
-    */
+    /// @note If not dynamic_constraints_ignorable, the linear distance should cause
+    /// problems.
     const auto [distance_to_front_waypoint, remaining_time_to_front_waypoint] = std::make_tuple(
       distance_along_lanelet(position, target_position),
       (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) +
         polyline_trajectory.shape.vertices.front().time - entity_status.time);
-    /*
-       This clause is to avoid division-by-zero errors in later clauses with
-       distance_to_front_waypoint as the denominator if the distance
-       miraculously becomes zero.
-    */
+    /// @note This clause is to avoid division-by-zero errors in later clauses with
+    /// distance_to_front_waypoint as the denominator if the distance
+    /// miraculously becomes zero.
     isDefinitelyLessThan(distance_to_front_waypoint, std::numeric_limits<double>::epsilon())) {
     return discard_the_front_waypoint_and_recurse();
   } else if (
     const auto [distance, remaining_time] =
       [&]() {
-        /*
-           Note for anyone working on adding support for followingMode follow
-           to this function (FollowPolylineTrajectoryAction::tick) in the
-           future: if followingMode is follow, this distance calculation may be
-           inappropriate.
-        */
+        /// @note Note for anyone working on adding support for followingMode follow
+        /// to this function (FollowPolylineTrajectoryAction::tick) in the
+        /// future: if followingMode is follow, this distance calculation may be
+        /// inappropriate.
         auto total_distance_to = [&](auto last) {
           auto total_distance = 0.0;
           for (auto iter = std::begin(polyline_trajectory.shape.vertices);
@@ -221,28 +209,26 @@ auto makeUpdatedStatus(
                 (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time
                                                                : 0.0) +
                 first_waypoint_with_arrival_time_specified()->time - entity_status.time;
-              /*
-                 The condition below should ideally be remaining_time < 0.
-
-                 The simulator runs at a constant frame rate, so the step time is
-                 1/FPS. If the simulation time is an accumulation of step times
-                 expressed as rational numbers, times that are integer multiples
-                 of the frame rate will always be exact integer seconds.
-                 Therefore, the timing of remaining_time == 0 always exists, and
-                 the velocity planning of this member function (tick) aims to
-                 reach the waypoint exactly at that timing. So the ideal timeout
-                 condition is remaining_time < 0.
-
-                 But actually the step time is expressed as a float and the
-                 simulation time is its accumulation. As a result, it is not
-                 guaranteed that there will be times when the simulation time is
-                 exactly zero. For example, remaining_time == -0.00006 and it was
-                 judged to be out of time.
-
-                 For the above reasons, the condition is remaining_time <
-                 -step_time. In other words, the conditions are such that a delay
-                 of 1 step time is allowed.
-              */
+              /// @note The condition below should ideally be remaining_time < 0.
+              ///
+              /// The simulator runs at a constant frame rate, so the step time is
+              /// 1/FPS. If the simulation time is an accumulation of step times
+              /// expressed as rational numbers, times that are integer multiples
+              /// of the frame rate will always be exact integer seconds.
+              /// Therefore, the timing of remaining_time == 0 always exists, and
+              /// the velocity planning of this member function (tick) aims to
+              /// reach the waypoint exactly at that timing. So the ideal timeout
+              /// condition is remaining_time < 0.
+              ///
+              /// But actually the step time is expressed as a float and the
+              /// simulation time is its accumulation. As a result, it is not
+              /// guaranteed that there will be times when the simulation time is
+              /// exactly zero. For example, remaining_time == -0.00006 and it was
+              /// judged to be out of time.
+              ///
+              /// For the above reasons, the condition is remaining_time <
+              /// -step_time. In other words, the conditions are such that a delay
+              /// of 1 step time is allowed.
               remaining_time < -step_time) {
             throw common::Error(
               "Vehicle ", std::quoted(entity_status.name),
@@ -304,40 +290,36 @@ auto makeUpdatedStatus(
       std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", speed,
       ". ");
   } else if (
-    /*
-       The controller provides the ability to calculate acceleration using constraints from the
-       behavior_parameter. The value is_breaking_waypoint() determines whether the calculated
-       acceleration takes braking into account - it is true if the nearest waypoint with the
-       specified time is the last waypoint or the nearest waypoint without the specified time is the
-       last waypoint.
-
-       If an arrival time was specified for any of the remaining waypoints, priority is given to
-       meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can
-       be met.
-
-       However, the controller allows passing target_speed as a speed which is followed by the
-       controller. target_speed is passed only if no arrival time was specified for any of the
-       remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is
-       not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the
-       behaviour_parameter.
-    */
+    /// @note The controller provides the ability to calculate acceleration using constraints from the
+    /// behavior_parameter. The value is_breaking_waypoint() determines whether the calculated
+    /// acceleration takes braking into account - it is true if the nearest waypoint with the
+    /// specified time is the last waypoint or the nearest waypoint without the specified time is the
+    /// last waypoint.
+    ///
+    /// If an arrival time was specified for any of the remaining waypoints, priority is given to
+    /// meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can
+    /// be met.
+    ///
+    /// However, the controller allows passing target_speed as a speed which is followed by the
+    /// controller. target_speed is passed only if no arrival time was specified for any of the
+    /// remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is
+    /// not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the
+    /// behaviour_parameter.
     const auto follow_waypoint_controller = FollowWaypointController(
       behavior_parameter, step_time, is_breaking_waypoint(),
       std::isinf(remaining_time) ? target_speed : std::nullopt);
     false) {
   } else if (
-    /*
-       The desired acceleration is the acceleration at which the destination
-       can be reached exactly at the specified time (= time remaining at zero).
-
-       The desired acceleration is calculated to the nearest waypoint with a specified arrival time.
-       It is calculated in such a way as to reach a constant linear speed as quickly as possible,
-       ensuring arrival at a waypoint at the precise time and with the shortest possible distance.
-       More precisely, the controller selects acceleration to minimize the distance to the waypoint
-       that will be reached in a time step defined as the expected arrival time.
-       In addition, the controller ensures a smooth stop at the last waypoint of the trajectory,
-       with linear speed equal to zero and acceleration equal to zero.
-    */
+    /// @note The desired acceleration is the acceleration at which the destination
+    /// can be reached exactly at the specified time (= time remaining at zero).
+    ///
+    /// The desired acceleration is calculated to the nearest waypoint with a specified arrival time.
+    /// It is calculated in such a way as to reach a constant linear speed as quickly as possible,
+    /// ensuring arrival at a waypoint at the precise time and with the shortest possible distance.
+    /// More precisely, the controller selects acceleration to minimize the distance to the waypoint
+    /// that will be reached in a time step defined as the expected arrival time.
+    /// In addition, the controller ensures a smooth stop at the last waypoint of the trajectory,
+    /// with linear speed equal to zero and acceleration equal to zero.
     const auto desired_acceleration = [&]() -> double {
       try {
         return follow_waypoint_controller.getAcceleration(
@@ -365,15 +347,13 @@ auto makeUpdatedStatus(
       desired_speed, ". ");
   } else if (const auto desired_velocity =
                [&]() {
-                 /*
-                    Note: The followingMode in OpenSCENARIO is passed as
-                    variable dynamic_constraints_ignorable. the value of the
-                    variable is `followingMode == position`.
-                 */
+                 /// @note The followingMode in OpenSCENARIO is passed as
+                 /// variable dynamic_constraints_ignorable. the value of the
+                 /// variable is `followingMode == position`.
                  if (polyline_trajectory.dynamic_constraints_ignorable) {
                    const auto dx = target_position.x - position.x;
                    const auto dy = target_position.y - position.y;
-                   // if entity is on lane use pitch from lanelet, otherwise use pitch on target
+                   /// @note if entity is on lane use pitch from lanelet, otherwise use pitch on target
                    const auto pitch =
                      entity_status.lanelet_pose_valid
                        ? -math::geometry::convertQuaternionToEulerAngle(
@@ -386,14 +366,12 @@ auto makeUpdatedStatus(
                      .y(std::cos(pitch) * std::sin(yaw) * desired_speed)
                      .z(std::sin(pitch) * desired_speed);
                  } else {
-                   /*
-                      Note: The vector returned if
-                      dynamic_constraints_ignorable == true ignores parameters
-                      such as the maximum rudder angle of the vehicle entry. In
-                      this clause, such parameters must be respected and the
-                      rotation angle difference of the z-axis center of the
-                      vector must be kept below a certain value.
-                   */
+                   /// @note The vector returned if
+                   /// dynamic_constraints_ignorable == true ignores parameters
+                   /// such as the maximum rudder angle of the vehicle entry. In
+                   /// this clause, such parameters must be respected and the
+                   /// rotation angle difference of the z-axis center of the
+                   /// vector must be kept below a certain value.
                    throw common::SimulationError(
                      "The followingMode is only supported for position.");
                  }
@@ -515,37 +493,29 @@ auto makeUpdatedStatus(
     }
 
     if (std::isnan(remaining_time_to_front_waypoint)) {
-      /*
-        If the nearest waypoint is arrived at in this step without a specific arrival time, it will
-        be considered as achieved
-      */
+      /// @note If the nearest waypoint is arrived at in this step without a specific arrival time, it will
+      /// be considered as achieved
       if (std::isinf(remaining_time) && polyline_trajectory.shape.vertices.size() == 1) {
-        /*
-          If the trajectory has only waypoints with unspecified time, the last one is followed using
-          maximum speed including braking - in this case accuracy of arrival is checked
-        */
+        /// @note If the trajectory has only waypoints with unspecified time, the last one is followed using
+        /// maximum speed including braking - in this case accuracy of arrival is checked
         if (follow_waypoint_controller.areConditionsOfArrivalMet(
               acceleration, speed, distance_to_front_waypoint)) {
           return discard_the_front_waypoint_and_recurse();
         }
       } else {
-        /*
-          If it is an intermediate waypoint with an unspecified time, the accuracy of the arrival is
-          irrelevant
-        */
+        /// @note If it is an intermediate waypoint with an unspecified time, the accuracy of the arrival is
+        /// irrelevant
         if (auto this_step_distance = (speed + desired_acceleration * step_time) * step_time;
             this_step_distance > distance_to_front_waypoint) {
           return discard_the_front_waypoint_and_recurse();
         }
       }
-      /*
-        If there is insufficient time left for the next calculation step.
-        The value of step_time/2 is compared, as the remaining time is affected by floating point
-        inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time -
-        1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value
-        here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next
-        step is possible (remaining_time_to_front_waypoint is almost zero).
-      */
+      /// @note If there is insufficient time left for the next calculation step.
+      /// The value of step_time/2 is compared, as the remaining time is affected by floating point
+      /// inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time -
+      /// 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value
+      /// here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next
+      /// step is possible (remaining_time_to_front_waypoint is almost zero).
     } else if (isDefinitelyLessThan(remaining_time_to_front_waypoint, step_time / 2.0)) {
       if (follow_waypoint_controller.areConditionsOfArrivalMet(
             acceleration, speed, distance_to_front_waypoint)) {
@@ -561,26 +531,24 @@ auto makeUpdatedStatus(
       }
     }
 
-    /*
-       Note: If obstacle avoidance is to be implemented, the steering behavior
-       known by the name "collision avoidance" should be synthesized here into
-       steering.
-    */
+    /// @note If obstacle avoidance is to be implemented, the steering behavior
+    /// known by the name "collision avoidance" should be synthesized here into
+    /// steering.
     auto updated_status = entity_status;
 
     updated_status.pose.position += desired_velocity * step_time;
 
     updated_status.pose.orientation = [&]() {
       if (desired_velocity.y == 0 && desired_velocity.x == 0 && desired_velocity.z == 0) {
-        // Do not change orientation if there is no designed_velocity vector
+        /// @note Do not change orientation if there is no designed_velocity vector
         return entity_status.pose.orientation;
       } else {
-        // if there is a designed_velocity vector, set the orientation in the direction of it
+        /// @note if there is a designed_velocity vector, set the orientation in the direction of it
         return math::geometry::convertDirectionToQuaternion(desired_velocity);
       }
     }();
 
-    // If it is the transition between lanelets: overwrite position to improve precision
+    /// @note If it is the transition between lanelets: overwrite position to improve precision
     if (entity_status.lanelet_pose_valid) {
       constexpr bool desired_velocity_is_global{true};
       const auto canonicalized_lanelet_pose = traffic_simulator::pose::toCanonicalizedLaneletPose(
@@ -591,7 +559,7 @@ auto makeUpdatedStatus(
       if (canonicalized_lanelet_pose && estimated_next_canonicalized_lanelet_pose) {
         const auto next_lanelet_id =
           static_cast<LaneletPose>(estimated_next_canonicalized_lanelet_pose.value()).lanelet_id;
-        if (  // Handle lanelet transition
+        if (  /// @note Handle lanelet transition
           const auto updated_position = pose::updatePositionForLaneletTransition(
             canonicalized_lanelet_pose.value(), next_lanelet_id, desired_velocity,
             desired_velocity_is_global, step_time)) {
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index c4bae8cb08d..0904ee42f63 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -188,10 +188,10 @@ auto updatePositionForLaneletTransition(
 
   const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
 
-  // Determine the displacement in the 2D lanelet coordinate system
+  /// @note Determine the displacement in the 2D lanelet coordinate system
   Eigen::Vector2d displacement;
   if (desired_velocity_is_global) {
-    // Transform desired (global) velocity to local velocity
+    /// @note Transform desired (global) velocity to local velocity
     const auto map_pose = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose);
     const Eigen::Vector3d global_velocity(
       desired_velocity.x, desired_velocity.y, desired_velocity.z);
@@ -213,7 +213,7 @@ auto updatePositionForLaneletTransition(
   const auto next_lanelet_longitudinal_d = longitudinal_d - remaining_lanelet_length;
   if (longitudinal_d <= remaining_lanelet_length) {
     return std::nullopt;
-  } else if (  // if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible
+  } else if (  /// @note if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible
     next_lanelet_longitudinal_d < lanelet_wrapper::lanelet_map::laneletLength(next_lanelet_id)) {
     LaneletPose result_lanelet_pose;
     result_lanelet_pose.lanelet_id = next_lanelet_id;
@@ -290,8 +290,8 @@ auto relativeLaneletPose(
   constexpr bool include_opposite_direction{true};
 
   LaneletPose position = quietNaNLaneletPose();
-  // here the s and offset are intentionally assigned independently, even if
-  // it is not possible to calculate one of them - it happens that one is sufficient
+  /// @note here the s and offset are intentionally assigned independently, even if
+  /// it is not possible to calculate one of them - it happens that one is sufficient
   if (
     const auto longitudinal_distance = longitudinalDistance(
       from, to, include_adjacent_lanelet, include_opposite_direction, routing_configuration,
@@ -319,8 +319,8 @@ auto boundingBoxRelativeLaneletPose(
   constexpr bool include_opposite_direction{true};
 
   LaneletPose position = quietNaNLaneletPose();
-  // here the s and offset are intentionally assigned independently, even if
-  // it is not possible to calculate one of them - it happens that one is sufficient
+  /// @note here the s and offset are intentionally assigned independently, even if
+  /// it is not possible to calculate one of them - it happens that one is sufficient
   if (
     const auto longitudinal_bounding_box_distance = boundingBoxLaneLongitudinalDistance(
       from, from_bounding_box, to, to_bounding_box, include_adjacent_lanelet,
@@ -386,11 +386,9 @@ auto isAtEndOfLanelets(
 
 namespace pedestrian
 {
-/*
-  This function has been moved from pedestrian_action_node and modified,
-  in case of inconsistency please compare in original:
-  https://github.com/tier4/scenario_simulator_v2/blob/090a8d08bcb065d293a530cf641a953edf311f9f/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L67-L128
-*/
+/// @note This function has been moved from pedestrian_action_node and modified,
+/// in case of inconsistency please compare in original:
+/// https://github.com/tier4/scenario_simulator_v2/blob/090a8d08bcb065d293a530cf641a953edf311f9f/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L67-L128
 auto transformToCanonicalizedLaneletPose(
   const geometry_msgs::msg::Pose & map_pose,
   const traffic_simulator_msgs::msg::BoundingBox & bounding_box,

From d38d3505126cf6e988f097124e1bf9a30101822b Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Mon, 27 Jan 2025 13:57:42 +0100
Subject: [PATCH 35/38] ref(traffic_simulator): Refactor comments

---
 .../src/behavior/follow_trajectory.cpp        | 216 ++++++++++--------
 .../traffic_simulator/src/utils/pose.cpp      |   8 +-
 2 files changed, 126 insertions(+), 98 deletions(-)

diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index eac06de886f..26f75ce79bf 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -101,21 +101,23 @@ auto makeUpdatedStatus(
   };
 
   auto discard_the_front_waypoint_and_recurse = [&]() {
-    /// @note The OpenSCENARIO standard does not define the behavior when the value of
-    /// Timing.domainAbsoluteRelative is "relative". The standard only states
-    /// "Definition of time value context as either absolute or relative", and
-    /// it is completely unclear when the relative time starts.
-    ///
-    /// This implementation has interpreted the specification as follows:
-    /// Relative time starts from the start of FollowTrajectoryAction or from
-    /// the time of reaching the previous "waypoint with arrival time".
-    ///
-    /// Note: not std::isnan(polyline_trajectory.base_time) means
-    /// "Timing.domainAbsoluteRelative is relative".
-    ///
-    /// Note: not std::isnan(polyline_trajectory.shape.vertices.front().time)
-    /// means "The waypoint about to be popped is the waypoint with the
-    /// specified arrival time".
+    /*
+       The OpenSCENARIO standard does not define the behavior when the value of
+       Timing.domainAbsoluteRelative is "relative". The standard only states
+       "Definition of time value context as either absolute or relative", and
+       it is completely unclear when the relative time starts.
+
+       This implementation has interpreted the specification as follows:
+       Relative time starts from the start of FollowTrajectoryAction or from
+       the time of reaching the previous "waypoint with arrival time".
+
+       Note: not std::isnan(polyline_trajectory.base_time) means
+       "Timing.domainAbsoluteRelative is relative".
+
+       Note: not std::isnan(polyline_trajectory.shape.vertices.front().time)
+       means "The waypoint about to be popped is the waypoint with the
+       specified arrival time".
+    */
     if (
       not std::isnan(polyline_trajectory.base_time) and
       not std::isnan(polyline_trajectory.shape.vertices.front().time)) {
@@ -148,11 +150,13 @@ auto makeUpdatedStatus(
            std::prev(polyline_trajectory.shape.vertices.end());
   };
 
-  /// @note The following code implements the steering behavior known as "seek". See
-  /// "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more
-  /// information.
-  ///
-  /// See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters
+  /*
+     The following code implements the steering behavior known as "seek". See
+     "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more
+     information.
+
+     See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters
+  */
   if (polyline_trajectory.shape.vertices.empty()) {
     return std::nullopt;
   } else if (const auto position = entity_status.pose.position; any(is_infinity_or_nan, position)) {
@@ -162,9 +166,11 @@ auto makeUpdatedStatus(
       std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [",
       position.x, ", ", position.y, ", ", position.z, "].");
   } else if (
-    /// @note We've made sure that polyline_trajectory.shape.vertices is not empty, so
-    /// a reference to vertices.front() always succeeds. vertices.front() is
-    /// referenced only this once in this member function.
+    /*
+       We've made sure that polyline_trajectory.shape.vertices is not empty, so
+       a reference to vertices.front() always succeeds. vertices.front() is
+       referenced only this once in this member function.
+    */
     const auto target_position = polyline_trajectory.shape.vertices.front().position.position;
     any(is_infinity_or_nan, target_position)) {
     throw common::Error(
@@ -174,24 +180,30 @@ auto makeUpdatedStatus(
       "'s target position coordinate value contains NaN or infinity. The value is [",
       target_position.x, ", ", target_position.y, ", ", target_position.z, "].");
   } else if (
-    /// @note If not dynamic_constraints_ignorable, the linear distance should cause
-    /// problems.
+    /*
+       If not dynamic_constraints_ignorable, the linear distance should cause
+       problems.
+    */
     const auto [distance_to_front_waypoint, remaining_time_to_front_waypoint] = std::make_tuple(
       distance_along_lanelet(position, target_position),
       (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) +
         polyline_trajectory.shape.vertices.front().time - entity_status.time);
-    /// @note This clause is to avoid division-by-zero errors in later clauses with
-    /// distance_to_front_waypoint as the denominator if the distance
-    /// miraculously becomes zero.
+    /*
+       This clause is to avoid division-by-zero errors in later clauses with
+       distance_to_front_waypoint as the denominator if the distance
+       miraculously becomes zero.
+    */
     isDefinitelyLessThan(distance_to_front_waypoint, std::numeric_limits<double>::epsilon())) {
     return discard_the_front_waypoint_and_recurse();
   } else if (
     const auto [distance, remaining_time] =
       [&]() {
-        /// @note Note for anyone working on adding support for followingMode follow
-        /// to this function (FollowPolylineTrajectoryAction::tick) in the
-        /// future: if followingMode is follow, this distance calculation may be
-        /// inappropriate.
+        /*
+           Note for anyone working on adding support for followingMode follow
+           to this function (FollowPolylineTrajectoryAction::tick) in the
+           future: if followingMode is follow, this distance calculation may be
+           inappropriate.
+        */
         auto total_distance_to = [&](auto last) {
           auto total_distance = 0.0;
           for (auto iter = std::begin(polyline_trajectory.shape.vertices);
@@ -209,26 +221,28 @@ auto makeUpdatedStatus(
                 (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time
                                                                : 0.0) +
                 first_waypoint_with_arrival_time_specified()->time - entity_status.time;
-              /// @note The condition below should ideally be remaining_time < 0.
-              ///
-              /// The simulator runs at a constant frame rate, so the step time is
-              /// 1/FPS. If the simulation time is an accumulation of step times
-              /// expressed as rational numbers, times that are integer multiples
-              /// of the frame rate will always be exact integer seconds.
-              /// Therefore, the timing of remaining_time == 0 always exists, and
-              /// the velocity planning of this member function (tick) aims to
-              /// reach the waypoint exactly at that timing. So the ideal timeout
-              /// condition is remaining_time < 0.
-              ///
-              /// But actually the step time is expressed as a float and the
-              /// simulation time is its accumulation. As a result, it is not
-              /// guaranteed that there will be times when the simulation time is
-              /// exactly zero. For example, remaining_time == -0.00006 and it was
-              /// judged to be out of time.
-              ///
-              /// For the above reasons, the condition is remaining_time <
-              /// -step_time. In other words, the conditions are such that a delay
-              /// of 1 step time is allowed.
+              /*
+                 The condition below should ideally be remaining_time < 0.
+
+                 The simulator runs at a constant frame rate, so the step time is
+                 1/FPS. If the simulation time is an accumulation of step times
+                 expressed as rational numbers, times that are integer multiples
+                 of the frame rate will always be exact integer seconds.
+                 Therefore, the timing of remaining_time == 0 always exists, and
+                 the velocity planning of this member function (tick) aims to
+                 reach the waypoint exactly at that timing. So the ideal timeout
+                 condition is remaining_time < 0.
+
+                 But actually the step time is expressed as a float and the
+                 simulation time is its accumulation. As a result, it is not
+                 guaranteed that there will be times when the simulation time is
+                 exactly zero. For example, remaining_time == -0.00006 and it was
+                 judged to be out of time.
+
+                 For the above reasons, the condition is remaining_time <
+                 -step_time. In other words, the conditions are such that a delay
+                 of 1 step time is allowed.
+              */
               remaining_time < -step_time) {
             throw common::Error(
               "Vehicle ", std::quoted(entity_status.name),
@@ -290,36 +304,40 @@ auto makeUpdatedStatus(
       std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", speed,
       ". ");
   } else if (
-    /// @note The controller provides the ability to calculate acceleration using constraints from the
-    /// behavior_parameter. The value is_breaking_waypoint() determines whether the calculated
-    /// acceleration takes braking into account - it is true if the nearest waypoint with the
-    /// specified time is the last waypoint or the nearest waypoint without the specified time is the
-    /// last waypoint.
-    ///
-    /// If an arrival time was specified for any of the remaining waypoints, priority is given to
-    /// meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can
-    /// be met.
-    ///
-    /// However, the controller allows passing target_speed as a speed which is followed by the
-    /// controller. target_speed is passed only if no arrival time was specified for any of the
-    /// remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is
-    /// not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the
-    /// behaviour_parameter.
+    /*
+       The controller provides the ability to calculate acceleration using constraints from the
+       behavior_parameter. The value is_breaking_waypoint() determines whether the calculated
+       acceleration takes braking into account - it is true if the nearest waypoint with the
+       specified time is the last waypoint or the nearest waypoint without the specified time is the
+       last waypoint.
+
+       If an arrival time was specified for any of the remaining waypoints, priority is given to
+       meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can
+       be met.
+
+       However, the controller allows passing target_speed as a speed which is followed by the
+       controller. target_speed is passed only if no arrival time was specified for any of the
+       remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is
+       not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the
+       behaviour_parameter.
+    */
     const auto follow_waypoint_controller = FollowWaypointController(
       behavior_parameter, step_time, is_breaking_waypoint(),
       std::isinf(remaining_time) ? target_speed : std::nullopt);
     false) {
   } else if (
-    /// @note The desired acceleration is the acceleration at which the destination
-    /// can be reached exactly at the specified time (= time remaining at zero).
-    ///
-    /// The desired acceleration is calculated to the nearest waypoint with a specified arrival time.
-    /// It is calculated in such a way as to reach a constant linear speed as quickly as possible,
-    /// ensuring arrival at a waypoint at the precise time and with the shortest possible distance.
-    /// More precisely, the controller selects acceleration to minimize the distance to the waypoint
-    /// that will be reached in a time step defined as the expected arrival time.
-    /// In addition, the controller ensures a smooth stop at the last waypoint of the trajectory,
-    /// with linear speed equal to zero and acceleration equal to zero.
+    /*
+       The desired acceleration is the acceleration at which the destination
+       can be reached exactly at the specified time (= time remaining at zero).
+
+       The desired acceleration is calculated to the nearest waypoint with a specified arrival time.
+       It is calculated in such a way as to reach a constant linear speed as quickly as possible,
+       ensuring arrival at a waypoint at the precise time and with the shortest possible distance.
+       More precisely, the controller selects acceleration to minimize the distance to the waypoint
+       that will be reached in a time step defined as the expected arrival time.
+       In addition, the controller ensures a smooth stop at the last waypoint of the trajectory,
+       with linear speed equal to zero and acceleration equal to zero.
+    */
     const auto desired_acceleration = [&]() -> double {
       try {
         return follow_waypoint_controller.getAcceleration(
@@ -347,9 +365,11 @@ auto makeUpdatedStatus(
       desired_speed, ". ");
   } else if (const auto desired_velocity =
                [&]() {
-                 /// @note The followingMode in OpenSCENARIO is passed as
-                 /// variable dynamic_constraints_ignorable. the value of the
-                 /// variable is `followingMode == position`.
+                 /*
+                    Note: The followingMode in OpenSCENARIO is passed as
+                    variable dynamic_constraints_ignorable. the value of the
+                    variable is `followingMode == position`.
+                 */
                  if (polyline_trajectory.dynamic_constraints_ignorable) {
                    const auto dx = target_position.x - position.x;
                    const auto dy = target_position.y - position.y;
@@ -366,12 +386,14 @@ auto makeUpdatedStatus(
                      .y(std::cos(pitch) * std::sin(yaw) * desired_speed)
                      .z(std::sin(pitch) * desired_speed);
                  } else {
-                   /// @note The vector returned if
-                   /// dynamic_constraints_ignorable == true ignores parameters
-                   /// such as the maximum rudder angle of the vehicle entry. In
-                   /// this clause, such parameters must be respected and the
-                   /// rotation angle difference of the z-axis center of the
-                   /// vector must be kept below a certain value.
+                   /*
+                      Note: The vector returned if
+                      dynamic_constraints_ignorable == true ignores parameters
+                      such as the maximum rudder angle of the vehicle entry. In
+                      this clause, such parameters must be respected and the
+                      rotation angle difference of the z-axis center of the
+                      vector must be kept below a certain value.
+                   */
                    throw common::SimulationError(
                      "The followingMode is only supported for position.");
                  }
@@ -510,12 +532,14 @@ auto makeUpdatedStatus(
           return discard_the_front_waypoint_and_recurse();
         }
       }
-      /// @note If there is insufficient time left for the next calculation step.
-      /// The value of step_time/2 is compared, as the remaining time is affected by floating point
-      /// inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time -
-      /// 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value
-      /// here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next
-      /// step is possible (remaining_time_to_front_waypoint is almost zero).
+      /*
+        If there is insufficient time left for the next calculation step.
+        The value of step_time/2 is compared, as the remaining time is affected by floating point
+        inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time -
+        1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value
+        here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next
+        step is possible (remaining_time_to_front_waypoint is almost zero).
+      */
     } else if (isDefinitelyLessThan(remaining_time_to_front_waypoint, step_time / 2.0)) {
       if (follow_waypoint_controller.areConditionsOfArrivalMet(
             acceleration, speed, distance_to_front_waypoint)) {
@@ -531,9 +555,11 @@ auto makeUpdatedStatus(
       }
     }
 
-    /// @note If obstacle avoidance is to be implemented, the steering behavior
-    /// known by the name "collision avoidance" should be synthesized here into
-    /// steering.
+    /*
+       Note: If obstacle avoidance is to be implemented, the steering behavior
+       known by the name "collision avoidance" should be synthesized here into
+       steering.
+    */
     auto updated_status = entity_status;
 
     updated_status.pose.position += desired_velocity * step_time;
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 0904ee42f63..55c0d1304d8 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -386,9 +386,11 @@ auto isAtEndOfLanelets(
 
 namespace pedestrian
 {
-/// @note This function has been moved from pedestrian_action_node and modified,
-/// in case of inconsistency please compare in original:
-/// https://github.com/tier4/scenario_simulator_v2/blob/090a8d08bcb065d293a530cf641a953edf311f9f/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L67-L128
+/*
+  This function has been moved from pedestrian_action_node and modified,
+  in case of inconsistency please compare in original:
+  https://github.com/tier4/scenario_simulator_v2/blob/090a8d08bcb065d293a530cf641a953edf311f9f/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L67-L128
+*/
 auto transformToCanonicalizedLaneletPose(
   const geometry_msgs::msg::Pose & map_pose,
   const traffic_simulator_msgs::msg::BoundingBox & bounding_box,

From 336a7eedabb7dc4a6a6e7f6d74dbbae987b3e212 Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Wed, 29 Jan 2025 00:14:28 +0100
Subject: [PATCH 36/38] fix(traffic_simulator) Fix an issue with follow
 trajectory action

 - Updated makeUpdatedStatus to fix an issue with toCanonicalizedLaneletPose
---
 .../traffic_simulator/src/behavior/follow_trajectory.cpp      | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 26f75ce79bf..9cfe77ab873 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -577,8 +577,8 @@ auto makeUpdatedStatus(
     /// @note If it is the transition between lanelets: overwrite position to improve precision
     if (entity_status.lanelet_pose_valid) {
       constexpr bool desired_velocity_is_global{true};
-      const auto canonicalized_lanelet_pose = traffic_simulator::pose::toCanonicalizedLaneletPose(
-        entity_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance);
+      const auto canonicalized_lanelet_pose =
+        traffic_simulator::pose::toCanonicalizedLaneletPose(entity_status.lanelet_pose);
       const auto estimated_next_canonicalized_lanelet_pose =
         traffic_simulator::pose::toCanonicalizedLaneletPose(
           updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance);

From 6d7aa6a7d841703c1a825abb5ac67aeccaf56077 Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Wed, 29 Jan 2025 15:04:56 +0100
Subject: [PATCH 37/38] fix(traffic_simulator): Fix an issue with lanelet
 transition

---
 .../traffic_simulator/src/behavior/follow_trajectory.cpp       | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 9cfe77ab873..196514fde23 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -580,8 +580,7 @@ auto makeUpdatedStatus(
       const auto canonicalized_lanelet_pose =
         traffic_simulator::pose::toCanonicalizedLaneletPose(entity_status.lanelet_pose);
       const auto estimated_next_canonicalized_lanelet_pose =
-        traffic_simulator::pose::toCanonicalizedLaneletPose(
-          updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance);
+        traffic_simulator::pose::toCanonicalizedLaneletPose(updated_status.pose, include_crosswalk);
       if (canonicalized_lanelet_pose && estimated_next_canonicalized_lanelet_pose) {
         const auto next_lanelet_id =
           static_cast<LaneletPose>(estimated_next_canonicalized_lanelet_pose.value()).lanelet_id;

From d2d20f6c9aa039ab6fe422fe73bd02558d53531b Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Tue, 4 Feb 2025 10:54:21 +0100
Subject: [PATCH 38/38] ref(behavior_tree_plugin): Refactored
 calculateUpdatedEntityStatusInWorldFrame

---
 simulation/behavior_tree_plugin/src/action_node.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index a364ae49458..5f26d0eca7b 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -518,7 +518,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
 
   const auto matching_distance = default_matching_distance_for_lanelet_pose_calculation;
 
-  const auto buildUpdatedPose =
+  const auto build_updated_pose =
     [&include_crosswalk, &matching_distance](
       const std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> & status,
       const geometry_msgs::msg::Twist & desired_twist, const double time_step) {
@@ -571,7 +571,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   const auto linear_jerk_new = std::get<2>(dynamics);
   const auto & accel_new = std::get<1>(dynamics);
   const auto & twist_new = std::get<0>(dynamics);
-  const auto pose_new = buildUpdatedPose(canonicalized_entity_status, twist_new, step_time);
+  const auto pose_new = build_updated_pose(canonicalized_entity_status, twist_new, step_time);
 
   auto entity_status_updated =
     static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status);