fix(traffic_simulator) Improve lanelet transition handling for entity…

… position updates

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;

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)

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();
+        }
       }
     }
 

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)