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>
@@ -511,30 +512,74 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   -> traffic_simulator::EntityStatus
 {
   using math::geometry::operator*;
+  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,
+      const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) {
+      geometry_msgs::msg::Pose updated_pose;
+
+      // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s)
+      geometry_msgs::msg::Vector3 delta_rotation;
+      delta_rotation.z = desired_twist.angular.z * time_step;
+      const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
+      updated_pose.orientation = status->getMapPose().orientation * delta_quaternion;
+
+      // apply position change
+      /// @todo first determine global desired_velocity, calculate position change using it
+      // then pass the same global desired_velocity to moveTowardsLaneletPose()
+      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;
+
+      // if it is the transition between lanelet pose: 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,
+            hdmap_utils_ptr);
+
+        // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same
+        if (estimated_next_canonicalized_lanelet_pose) {
+          const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose(
+            canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
+            desired_twist.linear, desired_velocity_is_global, time_step, hdmap_utils_ptr);
+          updated_pose.position =
+            traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position;
+        }
+      }
+      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(
     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, hdmap_utils);
+
   auto entity_status_updated =
     static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status);
   entity_status_updated.time = current_time + step_time;

simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp

@@ -63,7 +63,6 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
       return canonicalized_entity_status->getTwist().linear.x;
     }
   };
-
   if (getBlackBoardValues();
       request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or

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

@@ -60,6 +60,13 @@ auto transformRelativePoseToGlobal(
   const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose)
   -> geometry_msgs::msg::Pose;
 
+auto moveTowardsLaneletPose(
+  const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
+  const CanonicalizedLaneletPose & next_canonicalized_lanelet_pose,
+  const geometry_msgs::msg::Vector3 & desired_velocity, const bool desired_velocity_is_global,
+  const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+  -> LaneletPose;
+
 // 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 {
     if (const auto from_lanelet_pose =
@@ -569,6 +575,27 @@ auto makeUpdatedStatus(
       }
     }();
 
+    // if it is the transition between lanelet pose: overwrite position to improve precision
+    if (entity_status.lanelet_pose_valid) {
+      constexpr bool desired_velocity_is_global{true};
+
+      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);
+
+      const auto estimated_next_canonicalized_lanelet_pose =
+        traffic_simulator::pose::toCanonicalizedLaneletPose(
+          updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance,
+          hdmap_utils);
+
+      if (canonicalized_lanelet_pose && estimated_next_canonicalized_lanelet_pose) {
+        const auto next_lanelet_pose = pose::moveTowardsLaneletPose(
+          canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
+          desired_velocity, desired_velocity_is_global, step_time, hdmap_utils);
+        updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position;
+      }
+    }
+
     updated_status.action_status.twist.linear.x = norm(desired_velocity);
 
     updated_status.action_status.twist.linear.y = 0;

simulation/traffic_simulator/src/utils/pose.cpp

@@ -13,6 +13,11 @@
 // limitations under the License.
 
 #include <geometry/bounding_box.hpp>
+#include <geometry/quaternion/euler_to_quaternion.hpp>
+#include <geometry/quaternion/quaternion_to_euler.hpp>
+#include <geometry/vector3/norm.hpp>
+#include <geometry/vector3/normalize.hpp>
+#include <geometry/vector3/operator.hpp>
 #include <traffic_simulator/helper/helper.hpp>
 #include <traffic_simulator/utils/distance.hpp>
 #include <traffic_simulator/utils/pose.hpp>
@@ -133,6 +138,63 @@ auto transformRelativePoseToGlobal(
   return ret;
 }
 
+/// @note this function does not modify the orientation of LaneletPose
+// the orientation remains the same as in next_lanelet_pose
+auto moveTowardsLaneletPose(
+  const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
+  const CanonicalizedLaneletPose & next_canonicalized_lanelet_pose,
+  const geometry_msgs::msg::Vector3 & desired_velocity, const bool desired_velocity_is_global,
+  const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+  -> LaneletPose
+{
+  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 next_lanelet_pose = static_cast<LaneletPose>(next_canonicalized_lanelet_pose);
+
+  // determine the displacement in the 2D lanelet coordinate system
+  Eigen::Vector2d displacement;
+  if (desired_velocity_is_global) {
+    // transform desired (global) velocity to local velocity
+    const auto orientation =
+      static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose).orientation;
+    const Eigen::Vector3d global_velocity(
+      desired_velocity.x, desired_velocity.y, desired_velocity.z);
+    const Eigen::Quaterniond quaternion(orientation.w, orientation.x, orientation.y, orientation.z);
+    const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity;
+    displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
+                   Eigen::Vector2d(local_velocity.x(), local_velocity.y()) * step_time;
+  } else {
+    displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
+                   Eigen::Vector2d(desired_velocity.x, desired_velocity.y) * step_time;
+  }
+  const auto longitudinal_d = displacement.x();
+  const auto lateral_d = displacement.y();
+
+  LaneletPose result_lanelet_pose;
+  const auto remaining_lanelet_length =
+    hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s;
+  const auto next_lanelet_longitudinal_d = longitudinal_d - remaining_lanelet_length;
+  if (longitudinal_d <= remaining_lanelet_length) {
+    result_lanelet_pose.lanelet_id = lanelet_pose.lanelet_id;
+    result_lanelet_pose.s = lanelet_pose.s + longitudinal_d;
+  } else if (  // if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible
+    next_lanelet_longitudinal_d < hdmap_utils_ptr->getLaneletLength(next_lanelet_pose.lanelet_id)) {
+    result_lanelet_pose.lanelet_id = next_lanelet_pose.lanelet_id;
+    result_lanelet_pose.s = next_lanelet_longitudinal_d;
+  } else {
+    THROW_SIMULATION_ERROR(
+      "Next lanelet is too short: lanelet_id==", next_lanelet_pose.lanelet_id, " is shorter than ",
+      next_lanelet_longitudinal_d);
+  }
+  result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
+  result_lanelet_pose.rpy = lanelet_pose.rpy;
+  return result_lanelet_pose;
+}
+
 auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
   -> std::optional<geometry_msgs::msg::Pose>
 {