ref(traffic_simulator): fix pose::moveToTargetLaneletPose

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)

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 = [&]() {

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,21 +216,17 @@ 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);
       // 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;
+      pose_new.position = pose_new.position + displacement;
     }
   } else {
     pose_new.position += displacement;