diff --git a/common/math/geometry/include/geometry/quaternion/norm.hpp b/common/math/geometry/include/geometry/quaternion/norm.hpp new file mode 100644 index 00000000000..2ef8102b724 --- /dev/null +++ b/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 +#include + +namespace math +{ +namespace geometry +{ +template ::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_ diff --git a/common/math/geometry/include/geometry/quaternion/normalize.hpp b/common/math/geometry/include/geometry/quaternion/normalize.hpp new file mode 100644 index 00000000000..9b819b7bf3f --- /dev/null +++ b/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 +#include +#include +#include + +namespace math +{ +namespace geometry +{ +template ::value, std::nullptr_t> = nullptr> +auto normalize(const T & q) +{ + const auto n = norm(q); + if (std::fabs(n) <= std::numeric_limits::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::epsilon()); + } + return geometry_msgs::build().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_ diff --git a/common/math/geometry/include/geometry/quaternion/operator.hpp b/common/math/geometry/include/geometry/quaternion/operator.hpp index 624dd686852..ca124ed5e8d 100644 --- a/common/math/geometry/include/geometry/quaternion/operator.hpp +++ b/common/math/geometry/include/geometry/quaternion/operator.hpp @@ -16,6 +16,7 @@ #define GEOMETRY__QUATERNION__OPERATOR_HPP_ #include +#include #include 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, IsLikeVector3>, 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, IsLikeQuaternion>, std::nullptr_t> = diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 86db4b18d41..f3231594ffb 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -18,7 +18,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -511,6 +513,28 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( -> traffic_simulator::EntityStatus { using math::geometry::operator*; + using math::geometry::operator+; + using math::geometry::operator-; + using math::geometry::operator+=; + + const auto getLaneletPitch = [&](const lanelet::Id lanelet_id, const double s) { + const auto lanelet_orientation = hdmap_utils->toMapOrientation(lanelet_id, s); + const auto lanelet_rpy = math::geometry::convertQuaternionToEulerAngle(lanelet_orientation); + + return lanelet_rpy.y; + }; + + auto applyPitchToDisplacement = + [&](geometry_msgs::msg::Vector3 & displacement, const double pitch) { + const Eigen::Quaterniond pitch_rotation(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); + const Eigen::Vector3d rotated_displacement = + pitch_rotation * Eigen::Vector3d(displacement.x, displacement.y, displacement.z); + + displacement.x = rotated_displacement.x(); + displacement.y = rotated_displacement.y(); + displacement.z = rotated_displacement.z(); + }; + const auto speed_planner = traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( step_time, canonicalized_entity_status->getName()); @@ -520,21 +544,52 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( 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; + geometry_msgs::msg::Pose pose_new = canonicalized_entity_status->getMapPose(); + + const auto desired_velocity = geometry_msgs::build() + .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 & next_lanelet_id) { + const auto excess_displacement = + displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length; + + // Update position to the next lanelet if available, otherwise apply full displacement. + pose_new.position = next_lanelet_id + ? hdmap_utils->toMapPosition( + next_lanelet_id.value(), math::geometry::norm(excess_displacement)) + : pose_new.position + displacement; + + // Apply lateral offset if transitioning to the next lanelet + if (next_lanelet_id) { + pose_new.position.y += canonicalized_entity_status->getLaneletPose().offset; + } + }; + + 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 = + hdmap_utils->getLaneletLength(current_lanelet_Id) - lanelet_pose; + + // Adjust position if displacement exceeds the current lanelet length. + if (math::geometry::norm(displacement) > remaining_lanelet_length) { + const auto next_lanelet_ids = hdmap_utils->getNextLaneletIds(current_lanelet_Id); + adjustPositionAtLaneletBoundary( + remaining_lanelet_length, + next_lanelet_ids.empty() ? std::nullopt : std::make_optional(next_lanelet_ids[0])); + } else { + pose_new.position += displacement; + } + } + auto entity_status_updated = static_cast(*canonicalized_entity_status); entity_status_updated.time = current_time + step_time; diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 9be623a2cab..9b804fd8290 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -162,6 +162,16 @@ class HdMapUtils auto getAltitude(const traffic_simulator_msgs::msg::LaneletPose &) const -> double; + auto getNextLaneletOnRoute( + const lanelet::Id current_lanelet_id, const lanelet::Id destination_lanelet_id) const + -> std::optional; + + auto toMapPosition(const lanelet::Id lanelet_id, const double s) const + -> geometry_msgs::msg::Point; + + auto toMapOrientation(const lanelet::Id lanelet_id, const double s, const bool fill_pitch = true) + const -> geometry_msgs::msg::Quaternion; + auto getLaneChangeTrajectory( const geometry_msgs::msg::Pose & from, const traffic_simulator::lane_change::Parameter & lane_change_parameter, diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index da2ced4710c..d45b02688d7 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -551,8 +551,45 @@ auto makeUpdatedStatus( steering. */ auto updated_status = entity_status; - - updated_status.pose.position += desired_velocity * step_time; + const auto displacement = desired_velocity * step_time; + + auto adjustPositionAtLaneletBoundary = + [&](double remaining_lanelet_length, const std::optional & 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; + } 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.orientation = [&]() { if (desired_velocity.y == 0 && desired_velocity.x == 0 && desired_velocity.z == 0) { diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index eab04aaf2f2..de6e9c92f68 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1098,6 +1098,33 @@ auto HdMapUtils::getNextLaneletIds( return sortAndUnique(ids); } +auto HdMapUtils::getNextLaneletOnRoute( + const lanelet::Id current_lanelet_id, const lanelet::Id destination_lanelet_id) const + -> std::optional +{ + const auto route = getRoute(current_lanelet_id, destination_lanelet_id); + + if (const auto it = std::find(route.begin(), route.end(), current_lanelet_id); + it != route.end() && std::next(it) != route.end()) { + return *std::next(it); + } + + return std::nullopt; +} + +auto HdMapUtils::toMapPosition(const lanelet::Id lanelet_id, const double s) const + -> geometry_msgs::msg::Point +{ + return getCenterPointsSpline(lanelet_id)->getPoint(s); +} + +auto HdMapUtils::toMapOrientation( + const lanelet::Id lanelet_id, const double s, const bool fill_pitch) const + -> geometry_msgs::msg::Quaternion +{ + return getCenterPointsSpline(lanelet_id)->getPose(s, fill_pitch).orientation; +} + auto HdMapUtils::getTrafficLightIds() const -> lanelet::Ids { using namespace lanelet::utils::query;