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..8e83a7f364e --- /dev/null +++ b/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 +#include +#include +#include + +namespace math +{ +namespace geometry +{ +template ::value, std::nullptr_t> = nullptr> +auto normalize(const T & q) +{ + if (const auto n = norm(q); 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()); + } else { + 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/vector3/operator.hpp b/common/math/geometry/include/geometry/vector3/operator.hpp index f1d839b6cb1..79afece710a 100644 --- a/common/math/geometry/include/geometry/vector3/operator.hpp +++ b/common/math/geometry/include/geometry/vector3/operator.hpp @@ -15,6 +15,7 @@ #ifndef GEOMETRY__VECTOR3__OPERATOR_HPP_ #define GEOMETRY__VECTOR3__OPERATOR_HPP_ +#include #include #include #include @@ -24,6 +25,17 @@ namespace math { namespace geometry { +template < + typename T, std::enable_if_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, IsLikeVector3>, 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..438f5b31620 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -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 & status, + const geometry_msgs::msg::Twist & desired_twist, const double time_step, + const std::shared_ptr & 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(*canonicalized_entity_status); entity_status_updated.time = current_time + step_time; diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 8e722f5afb0..d0f65ea7c80 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/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("polyline_trajectory", polyline_trajectory) or diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 9dc3b8053f8..93b13227fd2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/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_ptr) + -> LaneletPose; + // Relative msg::Pose auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) -> std::optional; diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index da2ced4710c..ef9cbd8ea18 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -26,6 +26,7 @@ #include #include #include +#include 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; diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index b354ea0ea0f..a1b55266d3e 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -13,6 +13,11 @@ // limitations under the License. #include +#include +#include +#include +#include +#include #include #include #include @@ -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_ptr) + -> LaneletPose +{ + using math::geometry::operator*; + using math::geometry::operator+; + using math::geometry::operator-; + using math::geometry::operator+=; + + const auto lanelet_pose = static_cast(canonicalized_lanelet_pose); + const auto next_lanelet_pose = static_cast(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(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 {