ref(traffic_simulator): separete pose::moveToTargetPosition

tier4 · Dec 19, 2024 · 4ee1ebc · 4ee1ebc
1 parent 6a8f3ce
commit 4ee1ebc
Show file tree

Hide file tree

Showing 3 changed files with 66 additions and 37 deletions.
diff --git 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
@@ -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
@@ -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>
 {