fix(traffic_simulator): fix moveToLaneletPose in FollowTrajectoryAction

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)

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;

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;
   }