fix(behavior_tree_plugin) Fix isssue with pedestrian turning

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

@@ -0,0 +1,33 @@
+// 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__NORM_HPP_
+#define GEOMETRY__QUATERNION__NORM_HPP_
+
+#include <cmath>
+#include <geometry/quaternion/is_like_quaternion.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <typename T, std::enable_if_t<IsLikeQuaternion<T>::value, std::nullptr_t> = nullptr>
+auto norm(const T & q)
+{
+  return std::hypot(std::hypot(q.w, q.x), std::hypot(q.y, q.z));
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__NORM_HPP_

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

@@ -0,0 +1,43 @@
+// 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__NORMALIZE_HPP_
+#define GEOMETRY__QUATERNION__NORMALIZE_HPP_
+
+#include <geometry/quaternion/is_like_quaternion.hpp>
+#include <geometry/quaternion/norm.hpp>
+#include <geometry/quaternion/operator.hpp>
+#include <scenario_simulator_exception/exception.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <typename T, std::enable_if_t<IsLikeQuaternion<T>::value, std::nullptr_t> = nullptr>
+auto normalize(const T & q)
+{
+  const auto n = norm(q);
+  if (std::fabs(n) <= std::numeric_limits<double>::epsilon()) {
+    THROW_SIMULATION_ERROR(
+      "Norm of quaternion (", q.w, ",", q.x, ",", q.y, ",", q.z, ") is ", n,
+      ". The norm of the quaternion you want to normalize should be greater than ",
+      std::numeric_limits<double>::epsilon());
+  }
+  return geometry_msgs::build<geometry_msgs::msg::Quaternion>().x(q.x / n).y(q.y / n).z(q.z / n).w(
+    q.w / n);
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__NORMALIZE_HPP_

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

@@ -16,6 +16,7 @@
 #define GEOMETRY__QUATERNION__OPERATOR_HPP_
 
 #include <geometry/quaternion/is_like_quaternion.hpp>
+#include <geometry/vector3/vector3.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
 
 namespace math
@@ -64,6 +65,34 @@ auto operator*(const T & a, const U & b)
   return v;
 }
 
+template <
+  typename T, typename U,
+  std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>, IsLikeVector3<U>>, std::nullptr_t> =
+    nullptr>
+auto operator*(const T & a, const U & b)
+{
+  T b_quat;
+  b_quat.x = b.x;
+  b_quat.y = b.y;
+  b_quat.z = b.z;
+  b_quat.w = 0.0;
+
+  T a_inv = a;
+  a_inv.x = -a.x;
+  a_inv.y = -a.y;
+  a_inv.z = -a.z;
+  a_inv.w = a.w;
+
+  T result_quat = a * b_quat * a_inv;
+
+  U rotated_vector;
+  rotated_vector.x = result_quat.x;
+  rotated_vector.y = result_quat.y;
+  rotated_vector.z = result_quat.z;
+
+  return rotated_vector;
+}
+
 template <
   typename T, typename U,
   std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>, IsLikeQuaternion<U>>, std::nullptr_t> =

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -18,6 +18,7 @@
 #include <geometry/quaternion/euler_to_quaternion.hpp>
 #include <geometry/quaternion/get_rotation.hpp>
 #include <geometry/quaternion/get_rotation_matrix.hpp>
+#include <geometry/quaternion/normalize.hpp>
 #include <geometry/quaternion/quaternion_to_euler.hpp>
 #include <geometry/vector3/normalize.hpp>
 #include <memory>
@@ -544,13 +545,13 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   geometry_msgs::msg::Accel accel_new = std::get<1>(dynamics);
   geometry_msgs::msg::Twist twist_new = std::get<0>(dynamics);
   geometry_msgs::msg::Pose pose_new = canonicalized_entity_status->getMapPose();
-  const auto current_lanelet_Id = canonicalized_entity_status->getLaneletId();
 
   const auto desired_velocity = geometry_msgs::build<geometry_msgs::msg::Vector3>()
                                   .x(twist_new.linear.x)
                                   .y(twist_new.linear.y)
                                   .z(twist_new.linear.z);
   auto displacement = desired_velocity * step_time;
+  displacement = math::geometry::normalize(pose_new.orientation) * displacement;
 
   auto adjustPositionAtLaneletBoundary =
     [&](double remaining_lanelet_length, const std::optional<lanelet::Id> & next_lanelet_id) {
@@ -572,6 +573,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   if (!canonicalized_entity_status->laneMatchingSucceed()) {
     pose_new.position += displacement;
   } else {
+    const auto current_lanelet_Id = canonicalized_entity_status->getLaneletId();
     const auto lanelet_pose = canonicalized_entity_status->getLaneletPose().s;
     applyPitchToDisplacement(displacement, getLaneletPitch(current_lanelet_Id, lanelet_pose));
     const double remaining_lanelet_length =