Fix/slope inaccuracies

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,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__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)
+{
+  if (const auto n = norm(q); 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());
+  } else {
+    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/vector3/operator.hpp

@@ -15,6 +15,7 @@
 #ifndef GEOMETRY__VECTOR3__OPERATOR_HPP_
 #define GEOMETRY__VECTOR3__OPERATOR_HPP_
 
+#include <Eigen/Dense>
 #include <geometry/vector3/is_like_vector3.hpp>
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
@@ -24,6 +25,17 @@ namespace math
 {
 namespace geometry
 {
+template <
+  typename T, std::enable_if_t<std::conjunction_v<IsLikeVector3<T>>, std::nullptr_t> = nullptr>
+auto operator+(const T & v, const Eigen::Vector3d & eigen_v) -> T
+{
+  T result;
+  result.x = v.x + eigen_v.x();
+  result.y = v.y + eigen_v.y();
+  result.z = v.z + eigen_v.z();
+  return result;
+}
+
 template <
   typename T, typename U,
   std::enable_if_t<std::conjunction_v<IsLikeVector3<T>, IsLikeVector3<U>>, std::nullptr_t> =

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -19,6 +19,7 @@
 #include <geometry/quaternion/get_rotation.hpp>
 #include <geometry/quaternion/get_rotation_matrix.hpp>
 #include <geometry/quaternion/quaternion_to_euler.hpp>
+#include <geometry/vector3/operator.hpp>
 #include <memory>
 #include <optional>
 #include <rclcpp/rclcpp.hpp>
@@ -253,9 +254,7 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf
     const auto quat = math::geometry::getRotation(
       canonicalized_entity_status->getMapPose().orientation,
       other_entity_status.at(each.first).getMapPose().orientation);
-    /**
-     * @note hard-coded parameter, if the Yaw value of RPY is in ~1.5708 -> 1.5708, entity is a candidate of front entity.
-     */
+    /// @note hard-coded parameter, if the Yaw value of RPY is in [-pi/2, pi/2], entity is a candidate of front entity.
     if (
       std::fabs(math::geometry::convertQuaternionToEulerAngle(quat).z) <=
       boost::math::constants::half_pi<double>()) {
@@ -463,30 +462,73 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   -> traffic_simulator::EntityStatus
 {
   using math::geometry::operator*;
+  using math::geometry::operator+;
+  using math::geometry::operator+=;
+
+  constexpr bool desired_velocity_is_global{false};
+
+  const auto include_crosswalk = [](const auto & entity_type) {
+    return (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == entity_type.type) ||
+           (traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == entity_type.type);
+  }(canonicalized_entity_status->getType());
+
+  const auto matching_distance = default_matching_distance_for_lanelet_pose_calculation;
+
+  const auto buildUpdatedPose =
+    [&include_crosswalk, &matching_distance](
+      const std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> & status,
+      const geometry_msgs::msg::Twist & desired_twist, const double time_step) {
+      geometry_msgs::msg::Pose updated_pose;
+
+      /// @note Apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s)
+      geometry_msgs::msg::Vector3 delta_rotation;
+      delta_rotation = desired_twist.angular * time_step;
+      const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
+      updated_pose.orientation = status->getMapPose().orientation * delta_quaternion;
+
+      /// @note Apply position change
+      /// @todo first determine global desired_velocity, calculate position change using it
+      /// then pass the same global desired_velocity to updatePositionForLaneletTransition()
+      const Eigen::Matrix3d rotation_matrix =
+        math::geometry::getRotationMatrix(updated_pose.orientation);
+      const auto translation = Eigen::Vector3d(
+        desired_twist.linear.x * time_step, desired_twist.linear.y * time_step,
+        desired_twist.linear.z * time_step);
+      const Eigen::Vector3d delta_position = rotation_matrix * translation;
+      updated_pose.position = status->getMapPose().position + delta_position;
+
+      /// @note If it is the transition between lanelets: overwrite position to improve precision
+      if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
+        const auto estimated_next_canonicalized_lanelet_pose =
+          traffic_simulator::pose::toCanonicalizedLaneletPose(
+            updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance);
+        if (estimated_next_canonicalized_lanelet_pose) {
+          const auto next_lanelet_id = static_cast<traffic_simulator::LaneletPose>(
+                                         estimated_next_canonicalized_lanelet_pose.value())
+                                         .lanelet_id;
+          if (  /// @note Handle lanelet transition
+            const auto updated_position =
+              traffic_simulator::pose::updatePositionForLaneletTransition(
+                canonicalized_lanelet_pose.value(), next_lanelet_id, desired_twist.linear,
+                desired_velocity_is_global, time_step)) {
+            updated_pose.position = updated_position.value();
+          }
+        }
+      }
+      return updated_pose;
+    };
+
   const auto speed_planner =
     traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner(
       step_time, canonicalized_entity_status->getName());
   const auto dynamics = speed_planner.getDynamicStates(
     local_target_speed, constraints, canonicalized_entity_status->getTwist(),
     canonicalized_entity_status->getAccel());
-  double linear_jerk_new = std::get<2>(dynamics);
-  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;
-  geometry_msgs::msg::Vector3 angular_trans_vec;
-  angular_trans_vec.z = twist_new.angular.z * step_time;
-  geometry_msgs::msg::Quaternion angular_trans_quat =
-    math::geometry::convertEulerAngleToQuaternion(angular_trans_vec);
-  pose_new.orientation = canonicalized_entity_status->getMapPose().orientation * angular_trans_quat;
-  Eigen::Vector3d trans_vec;
-  trans_vec(0) = twist_new.linear.x * step_time;
-  trans_vec(1) = twist_new.linear.y * step_time;
-  trans_vec(2) = 0;
-  Eigen::Matrix3d rotation_mat = math::geometry::getRotationMatrix(pose_new.orientation);
-  trans_vec = rotation_mat * trans_vec;
-  pose_new.position.x = trans_vec(0) + canonicalized_entity_status->getMapPose().position.x;
-  pose_new.position.y = trans_vec(1) + canonicalized_entity_status->getMapPose().position.y;
-  pose_new.position.z = trans_vec(2) + canonicalized_entity_status->getMapPose().position.z;
+  const auto linear_jerk_new = std::get<2>(dynamics);
+  const auto & accel_new = std::get<1>(dynamics);
+  const auto & twist_new = std::get<0>(dynamics);
+  const auto pose_new = buildUpdatedPose(canonicalized_entity_status, twist_new, step_time);
+
   auto entity_status_updated =
     static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status);
   entity_status_updated.time = current_time + step_time;

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

@@ -60,6 +60,11 @@ auto transformRelativePoseToGlobal(
   const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose)
   -> geometry_msgs::msg::Pose;
 
+auto updatePositionForLaneletTransition(
+  const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id next_lanelet_id,
+  const geometry_msgs::msg::Vector3 & desired_velocity, const bool desired_velocity_is_global,
+  const double step_time) -> std::optional<geometry_msgs::msg::Point>;
+
 // Relative msg::Pose
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
   -> std::optional<geometry_msgs::msg::Pose>;

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,11 @@ auto makeUpdatedStatus(
   using math::geometry::normalize;
   using math::geometry::truncate;
 
+  const auto include_crosswalk = [](const auto & entity_type) {
+    return (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == entity_type.type) ||
+           (traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == entity_type.type);
+  }(entity_status.type);
+
   auto distance_along_lanelet =
     [&](const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double {
     const auto quaternion = math::geometry::convertDirectionToQuaternion(
@@ -367,7 +373,7 @@ auto makeUpdatedStatus(
                  if (polyline_trajectory.dynamic_constraints_ignorable) {
                    const auto dx = target_position.x - position.x;
                    const auto dy = target_position.y - position.y;
-                   // if entity is on lane use pitch from lanelet, otherwise use pitch on target
+                   /// @note if entity is on lane use pitch from lanelet, otherwise use pitch on target
                    const auto pitch =
                      entity_status.lanelet_pose_valid
                        ? -math::geometry::convertQuaternionToEulerAngle(
@@ -509,24 +515,18 @@ auto makeUpdatedStatus(
     }
 
     if (std::isnan(remaining_time_to_front_waypoint)) {
-      /*
-        If the nearest waypoint is arrived at in this step without a specific arrival time, it will
-        be considered as achieved
-      */
+      /// @note If the nearest waypoint is arrived at in this step without a specific arrival time, it will
+      /// be considered as achieved
       if (std::isinf(remaining_time) && polyline_trajectory.shape.vertices.size() == 1) {
-        /*
-          If the trajectory has only waypoints with unspecified time, the last one is followed using
-          maximum speed including braking - in this case accuracy of arrival is checked
-        */
+        /// @note If the trajectory has only waypoints with unspecified time, the last one is followed using
+        /// maximum speed including braking - in this case accuracy of arrival is checked
         if (follow_waypoint_controller.areConditionsOfArrivalMet(
               acceleration, speed, distance_to_front_waypoint)) {
           return discard_the_front_waypoint_and_recurse();
         }
       } else {
-        /*
-          If it is an intermediate waypoint with an unspecified time, the accuracy of the arrival is
-          irrelevant
-        */
+        /// @note If it is an intermediate waypoint with an unspecified time, the accuracy of the arrival is
+        /// irrelevant
         if (auto this_step_distance = (speed + desired_acceleration * step_time) * step_time;
             this_step_distance > distance_to_front_waypoint) {
           return discard_the_front_waypoint_and_recurse();
@@ -566,14 +566,34 @@ auto makeUpdatedStatus(
 
     updated_status.pose.orientation = [&]() {
       if (desired_velocity.y == 0 && desired_velocity.x == 0 && desired_velocity.z == 0) {
-        // do not change orientation if there is no designed_velocity vector
+        /// @note Do not change orientation if there is no designed_velocity vector
         return entity_status.pose.orientation;
       } else {
-        // if there is a designed_velocity vector, set the orientation in the direction of it
+        /// @note if there is a designed_velocity vector, set the orientation in the direction of it
         return math::geometry::convertDirectionToQuaternion(desired_velocity);
       }
     }();
 
+    /// @note If it is the transition between lanelets: overwrite position to improve precision
+    if (entity_status.lanelet_pose_valid) {
+      constexpr bool desired_velocity_is_global{true};
+      const auto canonicalized_lanelet_pose =
+        traffic_simulator::pose::toCanonicalizedLaneletPose(entity_status.lanelet_pose);
+      const auto estimated_next_canonicalized_lanelet_pose =
+        traffic_simulator::pose::toCanonicalizedLaneletPose(
+          updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance);
+      if (canonicalized_lanelet_pose && estimated_next_canonicalized_lanelet_pose) {
+        const auto next_lanelet_id =
+          static_cast<LaneletPose>(estimated_next_canonicalized_lanelet_pose.value()).lanelet_id;
+        if (  /// @note Handle lanelet transition
+          const auto updated_position = pose::updatePositionForLaneletTransition(
+            canonicalized_lanelet_pose.value(), next_lanelet_id, desired_velocity,
+            desired_velocity_is_global, step_time)) {
+          updated_status.pose.position = updated_position.value();
+        }
+      }
+    }
+
     updated_status.action_status.twist.linear.x = norm(desired_velocity);
 
     updated_status.action_status.twist.linear.y = 0;