fix(traffic_simulator): fix follow_trajectory_action issue, add orien…

…tation to distance calc, remove toCanonicalizedLaneletPose(point...) because it can cause a another issues

common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp

@@ -0,0 +1,46 @@
+// 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__DIRECTION_TO_QUATERNION_HPP_
+#define GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_
+
+#include <cmath>
+#include <geometry/quaternion/euler_to_quaternion.hpp>
+#include <geometry/vector3/is_like_vector3.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+
+namespace math
+{
+namespace geometry
+{
+auto convertDirectionToQuaternion(const double dx, const double dy, const double dz)
+{
+  const auto euler_angles = geometry_msgs::build<geometry_msgs::msg::Vector3>()
+                              .x(0.0)
+                              .y(std::atan2(-dz, std::hypot(dx, dy)))
+                              .z(std::atan2(dy, dx));
+  return math::geometry::convertEulerAngleToQuaternion(euler_angles);
+}
+
+template <
+  typename T, std::enable_if_t<std::conjunction_v<IsLikeVector3<T>>, std::nullptr_t> = nullptr>
+auto convertDirectionToQuaternion(const T & v)
+{
+  return convertDirectionToQuaternion(v.x, v.y, v.z);
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_

simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp

@@ -45,11 +45,6 @@ auto toCanonicalizedLaneletPose(
   const geometry_msgs::msg::Pose & map_pose, const bool include_crosswalk)
   -> std::optional<CanonicalizedLaneletPose>;
 
-auto toCanonicalizedLaneletPose(
-  const geometry_msgs::msg::Point & map_point,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk,
-  const double matching_distance) -> std::optional<CanonicalizedLaneletPose>;
-
 auto toCanonicalizedLaneletPose(
   const geometry_msgs::msg::Pose & map_pose,
   const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk,

simulation/traffic_simulator/src/behavior/follow_trajectory.cpp

@@ -13,7 +13,7 @@
 // limitations under the License.
 
 #include <arithmetic/floating_point/comparison.hpp>
-#include <geometry/quaternion/euler_to_quaternion.hpp>
+#include <geometry/quaternion/direction_to_quaternion.hpp>
 #include <geometry/quaternion/get_rotation.hpp>
 #include <geometry/quaternion/quaternion_to_euler.hpp>
 #include <geometry/vector3/hypot.hpp>
@@ -68,11 +68,20 @@ auto makeUpdatedStatus(
 
   auto distance_along_lanelet =
     [&](const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double {
+    const auto quaternion = math::geometry::convertDirectionToQuaternion(
+      geometry_msgs::build<geometry_msgs::msg::Vector3>()
+        .x(to.x - from.x)
+        .y(to.y - from.y)
+        .z(to.z - from.z));
+    const auto from_pose =
+      geometry_msgs::build<geometry_msgs::msg::Pose>().position(from).orientation(quaternion);
+    const auto to_pose =
+      geometry_msgs::build<geometry_msgs::msg::Pose>().position(to).orientation(quaternion);
     if (const auto from_lanelet_pose = pose::toCanonicalizedLaneletPose(
-          from, entity_status.bounding_box, false, matching_distance);
+          from_pose, entity_status.bounding_box, false, matching_distance);
         from_lanelet_pose) {
       if (const auto to_lanelet_pose = pose::toCanonicalizedLaneletPose(
-            to, entity_status.bounding_box, false, matching_distance);
+            to_pose, entity_status.bounding_box, false, matching_distance);
           to_lanelet_pose) {
         if (const auto distance = hdmap_utils->getLongitudinalDistance(
               static_cast<LaneletPose>(from_lanelet_pose.value()),
@@ -561,12 +570,7 @@ auto makeUpdatedStatus(
         return entity_status.pose.orientation;
       } else {
         // 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)
-            .y(std::atan2(-desired_velocity.z, std::hypot(desired_velocity.x, desired_velocity.y)))
-            .z(std::atan2(desired_velocity.y, desired_velocity.x));
-        return math::geometry::convertEulerAngleToQuaternion(direction);
+        return math::geometry::convertDirectionToQuaternion(desired_velocity);
       }
     }();
 

simulation/traffic_simulator/src/utils/pose.cpp

@@ -134,17 +134,6 @@ auto toCanonicalizedLaneletPose(
   }
 }
 
-auto toCanonicalizedLaneletPose(
-  const geometry_msgs::msg::Point & map_point,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk,
-  const double matching_distance) -> std::optional<CanonicalizedLaneletPose>
-{
-  return toCanonicalizedLaneletPose(
-    geometry_msgs::build<geometry_msgs::msg::Pose>().position(map_point).orientation(
-      geometry_msgs::build<geometry_msgs::msg::Quaternion>().x(0).y(0).z(0).w(1)),
-    bounding_box, include_crosswalk, matching_distance);
-}
-
 auto toCanonicalizedLaneletPose(
   const geometry_msgs::msg::Pose & map_pose,
   const traffic_simulator_msgs::msg::BoundingBox & bounding_box,