From 8dfa732162df0a778107c06cde442f888d25d553 Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Wed, 18 Dec 2024 00:31:10 +0100 Subject: [PATCH 01/38] fix(traffic_simulator) Fix lanelet slope inaccuracies - follow trajectory action --- .../hdmap_utils/hdmap_utils.hpp | 10 +++++ .../src/behavior/follow_trajectory.cpp | 41 ++++++++++++++++++- .../src/hdmap_utils/hdmap_utils.cpp | 26 ++++++++++++ 3 files changed, 75 insertions(+), 2 deletions(-) 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..82cb9eb8d71 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<lanelet::Id>; + + 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 + -> 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<lanelet::Id> & 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..562a0d67d3a 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1098,6 +1098,32 @@ auto HdMapUtils::getNextLaneletIds( return sortAndUnique(ids); } +auto HdMapUtils::getNextLaneletOnRoute( + const lanelet::Id current_lanelet_id, const lanelet::Id destination_lanelet_id) const + -> std::optional<lanelet::Id> +{ + 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 + -> geometry_msgs::msg::Quaternion +{ + return getCenterPointsSpline(lanelet_id)->getPose(s, true).orientation; +} + auto HdMapUtils::getTrafficLightIds() const -> lanelet::Ids { using namespace lanelet::utils::query; From 0aae03561deadddb28f716c4c9e343caaa9cc8d8 Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Wed, 18 Dec 2024 09:18:42 +0100 Subject: [PATCH 02/38] fix(behavior_tree_plugin) Fix lanelet slope inaccuracies - walk straight action --- .../behavior_tree_plugin/src/action_node.cpp | 83 +++++++++++++++---- .../hdmap_utils/hdmap_utils.hpp | 4 +- .../src/hdmap_utils/hdmap_utils.cpp | 5 +- 3 files changed, 73 insertions(+), 19 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 86db4b18d41..3c69df7d004 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/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/normalize.hpp> #include <memory> #include <optional> #include <rclcpp/rclcpp.hpp> @@ -511,6 +512,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 +543,51 @@ 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 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; + + auto adjustPositionAtLaneletBoundary = + [&](double remaining_lanelet_length, const std::optional<lanelet::Id> & 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 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<traffic_simulator::EntityStatus>(*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 82cb9eb8d71..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 @@ -169,8 +169,8 @@ class HdMapUtils 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 - -> geometry_msgs::msg::Quaternion; + 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, diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 562a0d67d3a..de6e9c92f68 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1118,10 +1118,11 @@ auto HdMapUtils::toMapPosition(const lanelet::Id lanelet_id, const double s) con return getCenterPointsSpline(lanelet_id)->getPoint(s); } -auto HdMapUtils::toMapOrientation(const lanelet::Id lanelet_id, const double s) const +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, true).orientation; + return getCenterPointsSpline(lanelet_id)->getPose(s, fill_pitch).orientation; } auto HdMapUtils::getTrafficLightIds() const -> lanelet::Ids From a2131850c734639896a211b668c2433a2c24a0fa Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Wed, 18 Dec 2024 13:05:35 +0100 Subject: [PATCH 03/38] fix(behavior_tree_plugin) Fix isssue with pedestrian turning --- .../include/geometry/quaternion/norm.hpp | 33 ++++++++++++++ .../include/geometry/quaternion/normalize.hpp | 43 ++++++++++++++++++ .../include/geometry/quaternion/operator.hpp | 29 ++++++++++++ .../behavior_tree_plugin/src/action_node.cpp | 45 ++++++++++++------- 4 files changed, 135 insertions(+), 15 deletions(-) create mode 100644 common/math/geometry/include/geometry/quaternion/norm.hpp create mode 100644 common/math/geometry/include/geometry/quaternion/normalize.hpp 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 <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_ 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 <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_ 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 <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> = diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 3c69df7d004..a6019cf314b 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/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> @@ -516,23 +517,27 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( 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); + const auto getEntityYaw = [&](const geometry_msgs::msg::Quaternion & orientation) { + return math::geometry::convertQuaternionToEulerAngle(orientation).z; + }; - return lanelet_rpy.y; + const auto getLaneletPitch = [&](const lanelet::Id lanelet_id, const double s) { + return math::geometry::convertQuaternionToEulerAngle( + hdmap_utils->toMapOrientation(lanelet_id, s)) + .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); + auto applyRotationToDisplacement = [&]( + geometry_msgs::msg::Vector3 & displacement, + const double angle, const Eigen::Vector3d & axis) { + const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axis)); + const Eigen::Vector3d rotated_displacement = + rotation * Eigen::Vector3d(displacement.x, displacement.y, displacement.z); - displacement.x = rotated_displacement.x(); - displacement.y = rotated_displacement.y(); - displacement.z = rotated_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( @@ -544,7 +549,7 @@ 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(); + static bool is_yaw_applied = false; const auto desired_velocity = geometry_msgs::build<geometry_msgs::msg::Vector3>() .x(twist_new.linear.x) @@ -570,13 +575,23 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( }; if (!canonicalized_entity_status->laneMatchingSucceed()) { + displacement = math::geometry::normalize(pose_new.orientation) * displacement; 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)); + applyRotationToDisplacement( + displacement, getLaneletPitch(current_lanelet_Id, lanelet_pose), Eigen::Vector3d::UnitY()); const double remaining_lanelet_length = hdmap_utils->getLaneletLength(current_lanelet_Id) - lanelet_pose; + if (!is_yaw_applied) { + const double yaw = getEntityYaw(canonicalized_entity_status->getMapPose().orientation); + applyRotationToDisplacement(displacement, yaw, Eigen::Vector3d::UnitZ()); + is_yaw_applied = true; + } + // 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); From 1fbb1c198fc1662fb158740b801eed21370c8f3b Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Wed, 18 Dec 2024 15:09:25 +0100 Subject: [PATCH 04/38] refactor(behavior_tree_plugin): refactor action_node::calculateUpdatedEntityStatusInWorldFrame --- .../include/geometry/vector3/rotate.hpp | 59 +++++++++++++++++++ .../behavior_tree_plugin/src/action_node.cpp | 59 ++++++++----------- 2 files changed, 85 insertions(+), 33 deletions(-) create mode 100644 common/math/geometry/include/geometry/vector3/rotate.hpp diff --git a/common/math/geometry/include/geometry/vector3/rotate.hpp b/common/math/geometry/include/geometry/vector3/rotate.hpp new file mode 100644 index 00000000000..33648f01f83 --- /dev/null +++ b/common/math/geometry/include/geometry/vector3/rotate.hpp @@ -0,0 +1,59 @@ +// 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__VECTOR3__ROTATE_HPP_ +#define GEOMETRY__VECTOR3__ROTATE_HPP_ + +#include <geometry/vector3/is_like_vector3.hpp> +#include <scenario_simulator_exception/exception.hpp> + +namespace math +{ +namespace geometry +{ +inline Eigen::Vector3d axisToEigenAxis(Axis axis) +{ + switch (axis) { + case Axis::X: + return Eigen::Vector3d::UnitX(); + case Axis::Y: + return Eigen::Vector3d::UnitY(); + case Axis::Z: + return Eigen::Vector3d::UnitZ(); + default: + THROW_SIMULATION_ERROR("Invalid axis specified."); + } +} + +// Rotate a vector by a given angle around a specified axis +template < + typename T, std::enable_if_t<std::conjunction_v<IsLikeVector3<T>>, std::nullptr_t> = nullptr> +auto rotate(T & v, const double angle, const Axis axis) +{ + if (!std::isfinite(angle)) { + THROW_SIMULATION_ERROR("The provided angle for rotation is not finite."); + } + + const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axisToEigenAxis(axis))); + const Eigen::Vector3d eigen_vector(v.x, v.y, v.z); + const Eigen::Vector3d rotated_vector = rotation * eigen_vector; + + v.x = rotated_vector.x(); + v.y = rotated_vector.y(); + v.z = rotated_vector.z(); +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__VECTOR3__ROTATE_HPP_ diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index a6019cf314b..7048131138e 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -21,6 +21,7 @@ #include <geometry/quaternion/normalize.hpp> #include <geometry/quaternion/quaternion_to_euler.hpp> #include <geometry/vector3/normalize.hpp> +#include <geometry/vector3/rotate.hpp> #include <memory> #include <optional> #include <rclcpp/rclcpp.hpp> @@ -516,27 +517,26 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( using math::geometry::operator+; using math::geometry::operator-; using math::geometry::operator+=; + static bool is_yaw_applied = false; - const auto getEntityYaw = [&](const geometry_msgs::msg::Quaternion & orientation) { - return math::geometry::convertQuaternionToEulerAngle(orientation).z; - }; - - const auto getLaneletPitch = [&](const lanelet::Id lanelet_id, const double s) { - return math::geometry::convertQuaternionToEulerAngle( - hdmap_utils->toMapOrientation(lanelet_id, s)) - .y; + // Apply pitch rotation based on the lanelet's pitch angle + auto applyPitchRotation = [&](auto & disp, const double lanelet_pose_s) { + const auto current_lanelet_Id = canonicalized_entity_status->getLaneletId(); + const auto lanelet_pitch = math::geometry::convertQuaternionToEulerAngle( + hdmap_utils->toMapOrientation(current_lanelet_Id, lanelet_pose_s)) + .y; + math::geometry::rotate(disp, lanelet_pitch, math::geometry::Axis::Y); }; - auto applyRotationToDisplacement = [&]( - geometry_msgs::msg::Vector3 & displacement, - const double angle, const Eigen::Vector3d & axis) { - const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axis)); - const Eigen::Vector3d rotated_displacement = - rotation * Eigen::Vector3d(displacement.x, displacement.y, displacement.z); - - displacement.x = rotated_displacement.x(); - displacement.y = rotated_displacement.y(); - displacement.z = rotated_displacement.z(); + // Apply yaw rotation once to avoid cumulative lateral offset errors + auto applyYawRotation = [&](auto & disp) { + if (!is_yaw_applied) { + const auto yaw = math::geometry::convertQuaternionToEulerAngle( + canonicalized_entity_status->getMapPose().orientation) + .z; + math::geometry::rotate(disp, yaw, math::geometry::Axis::Z); + is_yaw_applied = true; + } }; const auto speed_planner = @@ -549,20 +549,19 @@ 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(); - static bool is_yaw_applied = false; - 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; + // Adjust the entity's position when approaching the end of the current lanelet auto adjustPositionAtLaneletBoundary = [&](double remaining_lanelet_length, const std::optional<lanelet::Id> & 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. + // If a next lanelet is available, transition to it; otherwise, apply displacement pose_new.position = next_lanelet_id ? hdmap_utils->toMapPosition( next_lanelet_id.value(), math::geometry::norm(excess_displacement)) @@ -577,22 +576,16 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( if (!canonicalized_entity_status->laneMatchingSucceed()) { displacement = math::geometry::normalize(pose_new.orientation) * displacement; pose_new.position += displacement; - } else { const auto current_lanelet_Id = canonicalized_entity_status->getLaneletId(); - const auto lanelet_pose = canonicalized_entity_status->getLaneletPose().s; - applyRotationToDisplacement( - displacement, getLaneletPitch(current_lanelet_Id, lanelet_pose), Eigen::Vector3d::UnitY()); - const double remaining_lanelet_length = - hdmap_utils->getLaneletLength(current_lanelet_Id) - lanelet_pose; + const auto lanelet_pose_s = canonicalized_entity_status->getLaneletPose().s; + const auto remaining_lanelet_length = + hdmap_utils->getLaneletLength(current_lanelet_Id) - lanelet_pose_s; - if (!is_yaw_applied) { - const double yaw = getEntityYaw(canonicalized_entity_status->getMapPose().orientation); - applyRotationToDisplacement(displacement, yaw, Eigen::Vector3d::UnitZ()); - is_yaw_applied = true; - } + applyPitchRotation(displacement, lanelet_pose_s); + applyYawRotation(displacement); - // Adjust position if displacement exceeds the current lanelet length. + // Check if the displacement exceeds the remaining lanelet length if (math::geometry::norm(displacement) > remaining_lanelet_length) { const auto next_lanelet_ids = hdmap_utils->getNextLaneletIds(current_lanelet_Id); adjustPositionAtLaneletBoundary( From 26f8a6547e0c4573a9ca4738e5486335a2c9e8b4 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 01:00:16 +0100 Subject: [PATCH 05/38] ref(geometry): add axis header, improve --- .../geometry/include/geometry/axis/axis.hpp | 41 ++++++++ .../include/geometry/polygon/polygon.hpp | 5 - .../include/geometry/quaternion/normalize.hpp | 13 ++- .../include/geometry/quaternion/operator.hpp | 30 +++--- .../include/geometry/vector3/rotate.hpp | 31 ++---- common/math/geometry/src/axis/axis.cpp | 94 +++++++++++++++++++ common/math/geometry/src/polygon/polygon.cpp | 67 ------------- .../test/src/polygon/test_polygon.cpp | 2 + .../primitives/primitive.hpp | 1 + 9 files changed, 170 insertions(+), 114 deletions(-) create mode 100644 common/math/geometry/include/geometry/axis/axis.hpp create mode 100644 common/math/geometry/src/axis/axis.cpp diff --git a/common/math/geometry/include/geometry/axis/axis.hpp b/common/math/geometry/include/geometry/axis/axis.hpp new file mode 100644 index 00000000000..e0da906d9f5 --- /dev/null +++ b/common/math/geometry/include/geometry/axis/axis.hpp @@ -0,0 +1,41 @@ + +// 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__AXIS__AXIS_HPP_ +#define GEOMETRY__AXIS__AXIS_HPP_ + +#include <Eigen/Dense> +#include <geometry_msgs/msg/point.hpp> + +namespace math +{ +namespace geometry +{ +enum class Axis { X = 0, Y = 1, Z = 2 }; + +auto axisToEigenAxis(Axis axis) -> Eigen::Vector3d; + +auto getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) + -> double; + +auto getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) + -> double; + +auto filterByAxis(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) + -> std::vector<double>; +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__AXIS__AXIS_HPP_ diff --git a/common/math/geometry/include/geometry/polygon/polygon.hpp b/common/math/geometry/include/geometry/polygon/polygon.hpp index 02f0374368c..231dae67ac6 100644 --- a/common/math/geometry/include/geometry/polygon/polygon.hpp +++ b/common/math/geometry/include/geometry/polygon/polygon.hpp @@ -21,11 +21,6 @@ namespace math { namespace geometry { -enum class Axis { X = 0, Y = 1, Z = 2 }; -double getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis); -double getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis); -std::vector<double> filterByAxis( - const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis); std::vector<geometry_msgs::msg::Point> get2DConvexHull( const std::vector<geometry_msgs::msg::Point> & points); } // namespace geometry diff --git a/common/math/geometry/include/geometry/quaternion/normalize.hpp b/common/math/geometry/include/geometry/quaternion/normalize.hpp index 9b819b7bf3f..8e83a7f364e 100644 --- a/common/math/geometry/include/geometry/quaternion/normalize.hpp +++ b/common/math/geometry/include/geometry/quaternion/normalize.hpp @@ -27,15 +27,18 @@ 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()) { + 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, + "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); } - 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 diff --git a/common/math/geometry/include/geometry/quaternion/operator.hpp b/common/math/geometry/include/geometry/quaternion/operator.hpp index ca124ed5e8d..4839a282333 100644 --- a/common/math/geometry/include/geometry/quaternion/operator.hpp +++ b/common/math/geometry/include/geometry/quaternion/operator.hpp @@ -69,26 +69,26 @@ 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) +auto operator*(const T & rotation, const U & vector) { - T b_quat; - b_quat.x = b.x; - b_quat.y = b.y; - b_quat.z = b.z; - b_quat.w = 0.0; + T vector_as_quaternion; + vector_as_quaternion.x = vector.x; + vector_as_quaternion.y = vector.y; + vector_as_quaternion.z = vector.z; + vector_as_quaternion.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 inverse_rotation = rotation; + inverse_rotation.x = -rotation.x; + inverse_rotation.y = -rotation.y; + inverse_rotation.z = -rotation.z; + inverse_rotation.w = rotation.w; - T result_quat = a * b_quat * a_inv; + T rotated_quaternion = rotation * vector_as_quaternion * inverse_rotation; U rotated_vector; - rotated_vector.x = result_quat.x; - rotated_vector.y = result_quat.y; - rotated_vector.z = result_quat.z; + rotated_vector.x = vector_as_quaternion.x; + rotated_vector.y = vector_as_quaternion.y; + rotated_vector.z = vector_as_quaternion.z; return rotated_vector; } diff --git a/common/math/geometry/include/geometry/vector3/rotate.hpp b/common/math/geometry/include/geometry/vector3/rotate.hpp index 33648f01f83..91c5b592bae 100644 --- a/common/math/geometry/include/geometry/vector3/rotate.hpp +++ b/common/math/geometry/include/geometry/vector3/rotate.hpp @@ -15,6 +15,7 @@ #ifndef GEOMETRY__VECTOR3__ROTATE_HPP_ #define GEOMETRY__VECTOR3__ROTATE_HPP_ +#include <geometry/axis/axis.hpp> #include <geometry/vector3/is_like_vector3.hpp> #include <scenario_simulator_exception/exception.hpp> @@ -22,20 +23,6 @@ namespace math { namespace geometry { -inline Eigen::Vector3d axisToEigenAxis(Axis axis) -{ - switch (axis) { - case Axis::X: - return Eigen::Vector3d::UnitX(); - case Axis::Y: - return Eigen::Vector3d::UnitY(); - case Axis::Z: - return Eigen::Vector3d::UnitZ(); - default: - THROW_SIMULATION_ERROR("Invalid axis specified."); - } -} - // Rotate a vector by a given angle around a specified axis template < typename T, std::enable_if_t<std::conjunction_v<IsLikeVector3<T>>, std::nullptr_t> = nullptr> @@ -43,15 +30,15 @@ auto rotate(T & v, const double angle, const Axis axis) { if (!std::isfinite(angle)) { THROW_SIMULATION_ERROR("The provided angle for rotation is not finite."); - } - - const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axisToEigenAxis(axis))); - const Eigen::Vector3d eigen_vector(v.x, v.y, v.z); - const Eigen::Vector3d rotated_vector = rotation * eigen_vector; + } else { + const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axisToEigenAxis(axis))); + const Eigen::Vector3d eigen_vector(v.x, v.y, v.z); + const Eigen::Vector3d rotated_vector = rotation * eigen_vector; - v.x = rotated_vector.x(); - v.y = rotated_vector.y(); - v.z = rotated_vector.z(); + v.x = rotated_vector.x(); + v.y = rotated_vector.y(); + v.z = rotated_vector.z(); + } } } // namespace geometry } // namespace math diff --git a/common/math/geometry/src/axis/axis.cpp b/common/math/geometry/src/axis/axis.cpp new file mode 100644 index 00000000000..422ddbbc36c --- /dev/null +++ b/common/math/geometry/src/axis/axis.cpp @@ -0,0 +1,94 @@ +// 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. + +#include <boost/geometry.hpp> +// #include <boost/geometry/geometries/point_xy.hpp> +// #include <boost/geometry/geometries/polygon.hpp> +// #include <geometry/polygon/polygon.hpp> +// #include <rclcpp/rclcpp.hpp> +#include <geometry/axis/axis.hpp> +#include <scenario_simulator_exception/exception.hpp> + +namespace math +{ +namespace geometry +{ +auto axisToEigenAxis(Axis axis) -> Eigen::Vector3d +{ + switch (axis) { + case Axis::X: + return Eigen::Vector3d::UnitX(); + case Axis::Y: + return Eigen::Vector3d::UnitY(); + case Axis::Z: + return Eigen::Vector3d::UnitZ(); + default: + THROW_SIMULATION_ERROR("Invalid axis specified."); + } +} + +auto getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) -> double +{ + if (const auto values = filterByAxis(points, axis); values.size() == 1) { + return values.front(); + } else { + return *std::max_element(values.begin(), values.end()); + } +} + +auto getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) -> double +{ + if (const auto values = filterByAxis(points, axis); values.size() == 1) { + return values.front(); + } else { + return *std::min_element(values.begin(), values.end()); + } +} + +auto filterByAxis(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) + -> std::vector<double> +{ + if (points.empty()) { + THROW_SIMULATION_ERROR( + "Invalid point list is specified, while trying to filter ", + axis == Axis::X ? "X" + : axis == Axis::Y ? "Y" + : "Z", + " axis. ", + "The point list in filterByAxis() should have at least one point to filter. " + "This message is not originally intended to be displayed, if you see it, please " + "contact the developer of traffic_simulator."); + } else { + const auto axisExtractor = + [](const Axis axis) -> std::function<double(const geometry_msgs::msg::Point &)> { + switch (axis) { + case Axis::X: + return [](const geometry_msgs::msg::Point & p) { return p.x; }; + case Axis::Y: + return [](const geometry_msgs::msg::Point & p) { return p.y; }; + case Axis::Z: + return [](const geometry_msgs::msg::Point & p) { return p.z; }; + default: + throw std::invalid_argument("Invalid axis"); + } + }; + + std::vector<double> single_axis; + std::transform( + points.begin(), points.end(), std::back_inserter(single_axis), axisExtractor(axis)); + return single_axis; + } +} +} // namespace geometry +} // namespace math diff --git a/common/math/geometry/src/polygon/polygon.cpp b/common/math/geometry/src/polygon/polygon.cpp index 7a7148b40d8..06886e6299a 100644 --- a/common/math/geometry/src/polygon/polygon.cpp +++ b/common/math/geometry/src/polygon/polygon.cpp @@ -47,72 +47,5 @@ std::vector<geometry_msgs::msg::Point> get2DConvexHull( } return polygon; } - -double getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) -{ - if (points.empty()) { - THROW_SIMULATION_ERROR( - "Invalid point list is specified, while getting max value on ", - axis == Axis::X ? "X" - : axis == Axis::Y ? "Y" - : "Z", - " axis. ", - "The point list in getMaxValue should have at least one point to get the max value from. " - "This message is not originally intended to be displayed, if you see it, please " - "contact the developer of traffic_simulator."); - } - const auto values = filterByAxis(points, axis); - if (values.size() == 1) { - return values.front(); - } - return *std::max_element(values.begin(), values.end()); -} - -double getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) -{ - if (points.empty()) { - THROW_SIMULATION_ERROR( - "Invalid point list is specified, while getting min value on ", - axis == Axis::X ? "X" - : axis == Axis::Y ? "Y" - : "Z", - " axis. ", - "The point list in getMinValue should have at least one point to get the min value from. " - "This message is not originally intended to be displayed, if you see it, please " - "contact the developer of traffic_simulator."); - } - const auto values = filterByAxis(points, axis); - if (values.size() == 1) { - return values.front(); - } - return *std::min_element(values.begin(), values.end()); -} - -std::vector<double> filterByAxis( - const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) -{ - std::vector<double> ret; - switch (axis) { - case Axis::X: { - std::transform( - points.begin(), points.end(), std::back_inserter(ret), - [](const geometry_msgs::msg::Point & point) { return point.x; }); - break; - } - case Axis::Y: { - std::transform( - points.begin(), points.end(), std::back_inserter(ret), - [](const geometry_msgs::msg::Point & point) { return point.y; }); - break; - } - case Axis::Z: { - std::transform( - points.begin(), points.end(), std::back_inserter(ret), - [](const geometry_msgs::msg::Point & point) { return point.z; }); - break; - } - } - return ret; -} } // namespace geometry } // namespace math diff --git a/common/math/geometry/test/src/polygon/test_polygon.cpp b/common/math/geometry/test/src/polygon/test_polygon.cpp index e7278fa6566..9b4596e9913 100644 --- a/common/math/geometry/test/src/polygon/test_polygon.cpp +++ b/common/math/geometry/test/src/polygon/test_polygon.cpp @@ -14,6 +14,7 @@ #include <gtest/gtest.h> +#include <geometry/axis/axis.hpp> #include <geometry/polygon/polygon.hpp> #include <scenario_simulator_exception/exception.hpp> @@ -41,6 +42,7 @@ TEST(Polygon, filterByAxis) EXPECT_DOUBLE_EQ(values_z[2], -3.0); } +/// @todo refactor test to throw exception TEST(Polygon, filterByAxisEmptyVector) { std::vector<geometry_msgs::msg::Point> points; diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp index c514d40247e..b9c6b9d2fb4 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp @@ -18,6 +18,7 @@ #include <embree4/rtcore.h> #include <algorithm> +#include <geometry/axis/axis.hpp> #include <geometry/polygon/polygon.hpp> #include <geometry_msgs/msg/pose.hpp> #include <optional> From 6a8f3ce1b66dce410483ca1656028f5822d45661 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 01:01:29 +0100 Subject: [PATCH 06/38] ref(behavior_tree_plugin, traffic_simulator): separate pose::moveAlongLanelet --- .../behavior_tree_plugin/src/action_node.cpp | 72 ++++--------------- .../include/traffic_simulator/utils/pose.hpp | 5 ++ .../traffic_simulator/src/utils/pose.cpp | 58 +++++++++++++++ 3 files changed, 75 insertions(+), 60 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 7048131138e..5f41ee43a11 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -519,26 +519,6 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( using math::geometry::operator+=; static bool is_yaw_applied = false; - // Apply pitch rotation based on the lanelet's pitch angle - auto applyPitchRotation = [&](auto & disp, const double lanelet_pose_s) { - const auto current_lanelet_Id = canonicalized_entity_status->getLaneletId(); - const auto lanelet_pitch = math::geometry::convertQuaternionToEulerAngle( - hdmap_utils->toMapOrientation(current_lanelet_Id, lanelet_pose_s)) - .y; - math::geometry::rotate(disp, lanelet_pitch, math::geometry::Axis::Y); - }; - - // Apply yaw rotation once to avoid cumulative lateral offset errors - auto applyYawRotation = [&](auto & disp) { - if (!is_yaw_applied) { - const auto yaw = math::geometry::convertQuaternionToEulerAngle( - canonicalized_entity_status->getMapPose().orientation) - .z; - math::geometry::rotate(disp, yaw, math::geometry::Axis::Z); - is_yaw_applied = true; - } - }; - const auto speed_planner = traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( step_time, canonicalized_entity_status->getName()); @@ -548,52 +528,24 @@ 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 = canonicalized_entity_status->getMapPose(); 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; - - // Adjust the entity's position when approaching the end of the current lanelet - auto adjustPositionAtLaneletBoundary = - [&](double remaining_lanelet_length, const std::optional<lanelet::Id> & next_lanelet_id) { - const auto excess_displacement = - displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length; - - // If a next lanelet is available, transition to it; otherwise, apply 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()) { - displacement = math::geometry::normalize(pose_new.orientation) * displacement; - pose_new.position += displacement; + geometry_msgs::msg::Pose pose_new; + if ( + const auto canonicalized_lanelet_pose = + canonicalized_entity_status->getCanonicalizedLaneletPose()) { + pose_new = traffic_simulator::pose::moveAlongLanelet( + canonicalized_lanelet_pose.value(), desired_velocity, step_time, !is_yaw_applied, + hdmap_utils); + is_yaw_applied = true; } else { - const auto current_lanelet_Id = canonicalized_entity_status->getLaneletId(); - const auto lanelet_pose_s = canonicalized_entity_status->getLaneletPose().s; - const auto remaining_lanelet_length = - hdmap_utils->getLaneletLength(current_lanelet_Id) - lanelet_pose_s; - - applyPitchRotation(displacement, lanelet_pose_s); - applyYawRotation(displacement); - - // Check if the displacement exceeds the remaining 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; - } + pose_new = canonicalized_entity_status->getMapPose(); + pose_new.position += + math::geometry::normalize(pose_new.orientation) * desired_velocity * step_time; + /// @todo orientation does not change? } auto entity_status_updated = diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 9dc3b8053f8..cc914de3e28 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/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 moveAlongLanelet( + const CanonicalizedLaneletPose & canonicalized_lanelet_pose, + const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw, + const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose; + // Relative msg::Pose auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) -> std::optional<geometry_msgs::msg::Pose>; diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index b354ea0ea0f..6c577abfc48 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 <geometry/bounding_box.hpp> +#include <geometry/quaternion/normalize.hpp> +#include <geometry/quaternion/quaternion_to_euler.hpp> +#include <geometry/vector3/normalize.hpp> +#include <geometry/vector3/operator.hpp> +#include <geometry/vector3/rotate.hpp> #include <traffic_simulator/helper/helper.hpp> #include <traffic_simulator/utils/distance.hpp> #include <traffic_simulator/utils/pose.hpp> @@ -133,6 +138,59 @@ auto transformRelativePoseToGlobal( return ret; } +/// @todo remove adjust yaw... figure out a better solution, adjust_yaw is local.. +auto moveAlongLanelet( + const CanonicalizedLaneletPose & canonicalized_lanelet_pose, + const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw, + const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose +{ + 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 remaining_lanelet_length = + hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s; + + auto pose_new = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose); + auto displacement = desired_velocity * step_time; + + // Apply pitch rotation based on the lanelet's pitch angle + const auto lanelet_pitch = + math::geometry::convertQuaternionToEulerAngle( + hdmap_utils_ptr->toMapOrientation(lanelet_pose.lanelet_id, lanelet_pose.s)) + .y; + math::geometry::rotate(displacement, lanelet_pitch, math::geometry::Axis::Y); + + // Apply yaw rotation once to avoid cumulative lateral offset errors + if (adjust_yaw) { + const auto yaw = math::geometry::convertQuaternionToEulerAngle(pose_new.orientation).z; + math::geometry::rotate(displacement, yaw, math::geometry::Axis::Z); + } + + // Check if the displacement exceeds the remaining lanelet length + if (math::geometry::norm(displacement) > remaining_lanelet_length) { + const auto excess_displacement = + displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length; + /// @todo why always first next_lanelet? (route?) + if (const auto next_lanelet_ids = hdmap_utils_ptr->getNextLaneletIds(lanelet_pose.lanelet_id); + !next_lanelet_ids.empty()) { + // transition to next lanelet + const auto s = math::geometry::norm(excess_displacement); + pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_ids[0], lanelet_pose.s); + // Apply lateral offset if transitioning to the next lanelet + pose_new.position.y += canonicalized_lanelet_pose.getLaneletPose().offset; + } else { + pose_new.position += displacement; + } + } else { + pose_new.position += displacement; + } + /// @todo orientation? + return pose_new; +} + auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) -> std::optional<geometry_msgs::msg::Pose> { From 4ee1ebcd46118130d35a5137684ed8752d439d87 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 01:02:21 +0100 Subject: [PATCH 07/38] ref(traffic_simulator): separete pose::moveToTargetPosition --- .../include/traffic_simulator/utils/pose.hpp | 8 ++- .../src/behavior/follow_trajectory.cpp | 49 +++++-------------- .../traffic_simulator/src/utils/pose.cpp | 46 +++++++++++++++++ 3 files changed, 66 insertions(+), 37 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index cc914de3e28..724e2a096e8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -64,7 +64,13 @@ auto moveAlongLanelet( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose; - + +auto moveToTargetPosition( + const CanonicalizedLaneletPose & canonicalized_lanelet_pose, + const geometry_msgs::msg::Point & target_position, + const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw, + const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose; + // Relative msg::Pose auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) -> std::optional<geometry_msgs::msg::Pose>; diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index d45b02688d7..69140e1fd12 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -551,44 +551,21 @@ auto makeUpdatedStatus( steering. */ auto updated_status = entity_status; - const auto displacement = desired_velocity * step_time; - - auto adjustPositionAtLaneletBoundary = - [&](double remaining_lanelet_length, const std::optional<lanelet::Id> & 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; + constexpr bool adjust_yaw{false}; + constexpr bool include_crosswalk{false}; + if (!entity_status.lanelet_pose_valid) { + updated_status.pose.position += desired_velocity * step_time; + } else if ( + 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)) { + updated_status.pose = pose::moveToTargetPosition( + canonicalized_lanelet_pose.value(), target_position, desired_velocity, step_time, + adjust_yaw, hdmap_utils); + /// @todo is the orientation changed in moveToTargetPosition? } 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.position += desired_velocity * step_time; } updated_status.pose.orientation = [&]() { diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 6c577abfc48..35c9a15ce52 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -191,6 +191,52 @@ auto moveAlongLanelet( return pose_new; } +/// @todo merge moveAlongLanelet and moveToTargetPosition or separate the common part +auto moveToTargetPosition( + const CanonicalizedLaneletPose & canonicalized_lanelet_pose, + const geometry_msgs::msg::Point & target_position, + const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw, + const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose +{ + 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 remaining_lanelet_length = + hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s; + + auto pose_new = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose); + const auto displacement = desired_velocity * step_time; + + // Adjust position if displacement exceeds the current lanelet length. + if (math::geometry::norm(displacement) > remaining_lanelet_length) { + const auto excess_displacement = + displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length; + /// @todo it can throw an exception... quite offten + // so using just target_position and toLaneletPose is a bad idea, you need to figure out another one + const auto target_lanelet_pose = hdmap_utils_ptr->toLaneletPose( + target_position, updated_status.bounding_box, false, matching_distance); + if ( + const auto next_lanelet_id = hdmap_utils_ptr->getNextLaneletOnRoute( + lanelet_pose.lanelet_id, target_lanelet_pose->lanelet_id)) { + // update position to the next lanelet + const auto s = math::geometry::norm(excess_displacement); + pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_id.value(), s); + // apply lateral offset if transitioning to the next lanelet + pose_new.position.y += lanelet_pose.offset; + } else { + // apply full displacement + pose_new.position = updated_status.pose.position + displacement; + } + } else { + pose_new.position += displacement; + } + /// @todo orientation? + return pose_new; +} + auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) -> std::optional<geometry_msgs::msg::Pose> { From f9d067e411437a062ad18a0815d3b86b8b0ed663 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 01:36:03 +0100 Subject: [PATCH 08/38] ref(traffic_simulator): fix pose::moveToTargetLaneletPose --- .../include/traffic_simulator/utils/pose.hpp | 8 ++--- .../src/behavior/follow_trajectory.cpp | 30 +++++++++++-------- .../traffic_simulator/src/utils/pose.cpp | 20 ++++++------- 3 files changed, 31 insertions(+), 27 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 724e2a096e8..bcba09bbd6b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -65,11 +65,11 @@ auto moveAlongLanelet( const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose; -auto moveToTargetPosition( +auto moveToTargetLaneletPose( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, - const geometry_msgs::msg::Point & target_position, - const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw, - const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose; + const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity, + const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) + -> geometry_msgs::msg::Pose; // Relative msg::Pose auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 69140e1fd12..17b489dd52b 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/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,8 @@ auto makeUpdatedStatus( using math::geometry::normalize; using math::geometry::truncate; + constexpr bool include_crosswalk{false}; + auto distance_along_lanelet = [&](const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double { if (const auto from_lanelet_pose = @@ -551,21 +554,24 @@ auto makeUpdatedStatus( steering. */ auto updated_status = entity_status; + updated_status.pose.position += desired_velocity * step_time; - constexpr bool adjust_yaw{false}; - constexpr bool include_crosswalk{false}; - if (!entity_status.lanelet_pose_valid) { - updated_status.pose.position += desired_velocity * step_time; - } else if ( + // optionally overwrite pose + /// @todo is the orientation changed in moveToTargetLaneletPose? + /// @todo target_lanelet_pose can be optional... quite offten so using just target_position and toLaneletPose is a bad idea, you need to figure out another idea (not so far target but the intermediate point) + if (entity_status.lanelet_pose_valid) { 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)) { - updated_status.pose = pose::moveToTargetPosition( - canonicalized_lanelet_pose.value(), target_position, desired_velocity, step_time, - adjust_yaw, hdmap_utils); - /// @todo is the orientation changed in moveToTargetPosition? - } else { - updated_status.pose.position += desired_velocity * step_time; + include_crosswalk, matching_distance, hdmap_utils); + + const auto target_lanelet_pose = hdmap_utils->toLaneletPose( + target_position, updated_status.bounding_box, include_crosswalk, matching_distance); + + if (canonicalized_lanelet_pose && target_lanelet_pose) { + updated_status.pose = pose::moveToTargetLaneletPose( + canonicalized_lanelet_pose.value(), target_lanelet_pose.value(), desired_velocity, + step_time, hdmap_utils); + } } updated_status.pose.orientation = [&]() { diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 35c9a15ce52..93ce0207aa0 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -156,6 +156,7 @@ auto moveAlongLanelet( auto pose_new = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose); auto displacement = desired_velocity * step_time; + /// @todo from this - these lines are just weird // Apply pitch rotation based on the lanelet's pitch angle const auto lanelet_pitch = math::geometry::convertQuaternionToEulerAngle( @@ -168,6 +169,7 @@ auto moveAlongLanelet( const auto yaw = math::geometry::convertQuaternionToEulerAngle(pose_new.orientation).z; math::geometry::rotate(displacement, yaw, math::geometry::Axis::Z); } + /// @todo to this - these lines are just weird // Check if the displacement exceeds the remaining lanelet length if (math::geometry::norm(displacement) > remaining_lanelet_length) { @@ -191,12 +193,12 @@ auto moveAlongLanelet( return pose_new; } -/// @todo merge moveAlongLanelet and moveToTargetPosition or separate the common part -auto moveToTargetPosition( +/// @todo merge moveAlongLanelet and moveToTargetLaneletPose or separate the common part +auto moveToTargetLaneletPose( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, - const geometry_msgs::msg::Point & target_position, - const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw, - const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose + const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity, + const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) + -> geometry_msgs::msg::Pose { using math::geometry::operator*; using math::geometry::operator+; @@ -214,13 +216,9 @@ auto moveToTargetPosition( if (math::geometry::norm(displacement) > remaining_lanelet_length) { const auto excess_displacement = displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length; - /// @todo it can throw an exception... quite offten - // so using just target_position and toLaneletPose is a bad idea, you need to figure out another one - const auto target_lanelet_pose = hdmap_utils_ptr->toLaneletPose( - target_position, updated_status.bounding_box, false, matching_distance); if ( const auto next_lanelet_id = hdmap_utils_ptr->getNextLaneletOnRoute( - lanelet_pose.lanelet_id, target_lanelet_pose->lanelet_id)) { + lanelet_pose.lanelet_id, target_lanelet_pose.lanelet_id)) { // update position to the next lanelet const auto s = math::geometry::norm(excess_displacement); pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_id.value(), s); @@ -228,7 +226,7 @@ auto moveToTargetPosition( pose_new.position.y += lanelet_pose.offset; } else { // apply full displacement - pose_new.position = updated_status.pose.position + displacement; + pose_new.position = pose_new.position + displacement; } } else { pose_new.position += displacement; From 72eea6f3580f3bd6b53681e61b737f72ae9e1c17 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 02:28:55 +0100 Subject: [PATCH 09/38] fix(traffic_simulator): fix moveToLaneletPose in FollowTrajectoryAction --- .../include/traffic_simulator/utils/pose.hpp | 2 +- .../src/behavior/follow_trajectory.cpp | 36 +++++++++---------- .../traffic_simulator/src/utils/pose.cpp | 24 +++++-------- 3 files changed, 28 insertions(+), 34 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index bcba09bbd6b..1152807dc91 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -65,7 +65,7 @@ auto moveAlongLanelet( const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose; -auto moveToTargetLaneletPose( +auto moveToLaneletPose( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 17b489dd52b..598ed6cccf4 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -554,25 +554,8 @@ auto makeUpdatedStatus( steering. */ auto updated_status = entity_status; - updated_status.pose.position += desired_velocity * step_time; - - // optionally overwrite pose - /// @todo is the orientation changed in moveToTargetLaneletPose? - /// @todo target_lanelet_pose can be optional... quite offten so using just target_position and toLaneletPose is a bad idea, you need to figure out another idea (not so far target but the intermediate point) - if (entity_status.lanelet_pose_valid) { - 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 target_lanelet_pose = hdmap_utils->toLaneletPose( - target_position, updated_status.bounding_box, include_crosswalk, matching_distance); - if (canonicalized_lanelet_pose && target_lanelet_pose) { - updated_status.pose = pose::moveToTargetLaneletPose( - canonicalized_lanelet_pose.value(), target_lanelet_pose.value(), desired_velocity, - step_time, hdmap_utils); - } - } + updated_status.pose.position += desired_velocity * step_time; updated_status.pose.orientation = [&]() { if (desired_velocity.y == 0 && desired_velocity.x == 0 && desired_velocity.z == 0) { @@ -589,6 +572,23 @@ auto makeUpdatedStatus( } }(); + // optionally overwrite pose + /// @todo is the orientation changed in moveToLaneletPose? + if (entity_status.lanelet_pose_valid) { + 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 next_lanelet_pose = hdmap_utils->toLaneletPose( + updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance); + + if (canonicalized_lanelet_pose && next_lanelet_pose) { + updated_status.pose = pose::moveToLaneletPose( + canonicalized_lanelet_pose.value(), next_lanelet_pose.value(), desired_velocity, + step_time, hdmap_utils); + } + } + 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 93ce0207aa0..b211a2b3068 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -182,6 +182,7 @@ auto moveAlongLanelet( const auto s = math::geometry::norm(excess_displacement); pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_ids[0], lanelet_pose.s); // Apply lateral offset if transitioning to the next lanelet + /// @todo offset is not the same as y... pose_new.position.y += canonicalized_lanelet_pose.getLaneletPose().offset; } else { pose_new.position += displacement; @@ -193,10 +194,10 @@ auto moveAlongLanelet( return pose_new; } -/// @todo merge moveAlongLanelet and moveToTargetLaneletPose or separate the common part -auto moveToTargetLaneletPose( +/// @todo merge moveAlongLanelet and moveToLaneletPose or separate the common part +auto moveToLaneletPose( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, - const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity, + const LaneletPose & next_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose { @@ -216,18 +217,11 @@ auto moveToTargetLaneletPose( if (math::geometry::norm(displacement) > remaining_lanelet_length) { const auto excess_displacement = displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length; - if ( - const auto next_lanelet_id = hdmap_utils_ptr->getNextLaneletOnRoute( - lanelet_pose.lanelet_id, target_lanelet_pose.lanelet_id)) { - // update position to the next lanelet - const auto s = math::geometry::norm(excess_displacement); - pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_id.value(), s); - // apply lateral offset if transitioning to the next lanelet - pose_new.position.y += lanelet_pose.offset; - } else { - // apply full displacement - pose_new.position = pose_new.position + displacement; - } + const auto s = math::geometry::norm(excess_displacement); + pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_pose.lanelet_id, s); + // apply lateral offset if transitioning to the next lanelet + /// @todo offset is not the same as y... + pose_new.position.y += lanelet_pose.offset; } else { pose_new.position += displacement; } From bccd3c5ea43a24aff90b06738a39f0ae44e7a7b7 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 10:45:37 +0100 Subject: [PATCH 10/38] fix(behavior_tree_plugin): use moveToLaneletPose in calculateUpdatedEntityStatusInWorldFrame --- .../behavior_tree_plugin/src/action_node.cpp | 45 +++++++++++++------ .../src/behavior/follow_trajectory.cpp | 5 ++- 2 files changed, 35 insertions(+), 15 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 5f41ee43a11..4f1813538df 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -517,7 +517,11 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( using math::geometry::operator+; using math::geometry::operator-; using math::geometry::operator+=; - static bool is_yaw_applied = 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 speed_planner = traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( @@ -528,24 +532,37 @@ 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); - 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); geometry_msgs::msg::Pose pose_new; + // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s) + geometry_msgs::msg::Vector3 delta_rotation; + delta_rotation.z = twist_new.angular.z * step_time; + const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation); + pose_new.orientation = canonicalized_entity_status->getMapPose().orientation * delta_quaternion; + // apply position change + const Eigen::Matrix3d rotation_matix = math::geometry::getRotationMatrix(pose_new.orientation); + const Eigen::Vector3d translation = + Eigen::Vector3d(twist_new.linear.x * step_time, twist_new.linear.y * step_time, 0.0); + const Eigen::Vector3d delta_position_eigen = rotation_matix * translation; + /// @todo allow: canonicalized_entity_status->getMapPose().position + delta_position_eigen + geometry_msgs::msg::Vector3 delta_position; + delta_position.x = delta_position_eigen.x(); + delta_position.y = delta_position_eigen.y(); + delta_position.z = delta_position_eigen.z(); + pose_new.position = canonicalized_entity_status->getMapPose().position + delta_position; + if ( const auto canonicalized_lanelet_pose = canonicalized_entity_status->getCanonicalizedLaneletPose()) { - pose_new = traffic_simulator::pose::moveAlongLanelet( - canonicalized_lanelet_pose.value(), desired_velocity, step_time, !is_yaw_applied, - hdmap_utils); - is_yaw_applied = true; - } else { - pose_new = canonicalized_entity_status->getMapPose(); - pose_new.position += - math::geometry::normalize(pose_new.orientation) * desired_velocity * step_time; - /// @todo orientation does not change? + const auto next_lanelet_pose = hdmap_utils->toLaneletPose( + pose_new, canonicalized_entity_status->getBoundingBox(), include_crosswalk, + default_matching_distance_for_lanelet_pose_calculation); + + if (next_lanelet_pose) { + pose_new = traffic_simulator::pose::moveToLaneletPose( + canonicalized_lanelet_pose.value(), next_lanelet_pose.value(), twist_new.linear, step_time, + hdmap_utils); + } } auto entity_status_updated = diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 598ed6cccf4..bcab441bd66 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -67,7 +67,10 @@ auto makeUpdatedStatus( using math::geometry::normalize; using math::geometry::truncate; - constexpr bool include_crosswalk{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); + }(entity_status.type); auto distance_along_lanelet = [&](const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double { From 8b9a76d55ee96b33734e0bf3a3947a21510a1d30 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 10:48:48 +0100 Subject: [PATCH 11/38] Revert "ref(geometry): add axis header, improve" This reverts commit 26f8a6547e0c4573a9ca4738e5486335a2c9e8b4. --- .../geometry/include/geometry/axis/axis.hpp | 41 -------- .../include/geometry/polygon/polygon.hpp | 5 + .../include/geometry/vector3/rotate.hpp | 31 ++++-- common/math/geometry/src/axis/axis.cpp | 94 ------------------- common/math/geometry/src/polygon/polygon.cpp | 67 +++++++++++++ .../test/src/polygon/test_polygon.cpp | 2 - .../primitives/primitive.hpp | 1 - 7 files changed, 94 insertions(+), 147 deletions(-) delete mode 100644 common/math/geometry/include/geometry/axis/axis.hpp delete mode 100644 common/math/geometry/src/axis/axis.cpp diff --git a/common/math/geometry/include/geometry/axis/axis.hpp b/common/math/geometry/include/geometry/axis/axis.hpp deleted file mode 100644 index e0da906d9f5..00000000000 --- a/common/math/geometry/include/geometry/axis/axis.hpp +++ /dev/null @@ -1,41 +0,0 @@ - -// 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__AXIS__AXIS_HPP_ -#define GEOMETRY__AXIS__AXIS_HPP_ - -#include <Eigen/Dense> -#include <geometry_msgs/msg/point.hpp> - -namespace math -{ -namespace geometry -{ -enum class Axis { X = 0, Y = 1, Z = 2 }; - -auto axisToEigenAxis(Axis axis) -> Eigen::Vector3d; - -auto getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) - -> double; - -auto getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) - -> double; - -auto filterByAxis(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) - -> std::vector<double>; -} // namespace geometry -} // namespace math - -#endif // GEOMETRY__AXIS__AXIS_HPP_ diff --git a/common/math/geometry/include/geometry/polygon/polygon.hpp b/common/math/geometry/include/geometry/polygon/polygon.hpp index 231dae67ac6..02f0374368c 100644 --- a/common/math/geometry/include/geometry/polygon/polygon.hpp +++ b/common/math/geometry/include/geometry/polygon/polygon.hpp @@ -21,6 +21,11 @@ namespace math { namespace geometry { +enum class Axis { X = 0, Y = 1, Z = 2 }; +double getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis); +double getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis); +std::vector<double> filterByAxis( + const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis); std::vector<geometry_msgs::msg::Point> get2DConvexHull( const std::vector<geometry_msgs::msg::Point> & points); } // namespace geometry diff --git a/common/math/geometry/include/geometry/vector3/rotate.hpp b/common/math/geometry/include/geometry/vector3/rotate.hpp index 91c5b592bae..33648f01f83 100644 --- a/common/math/geometry/include/geometry/vector3/rotate.hpp +++ b/common/math/geometry/include/geometry/vector3/rotate.hpp @@ -15,7 +15,6 @@ #ifndef GEOMETRY__VECTOR3__ROTATE_HPP_ #define GEOMETRY__VECTOR3__ROTATE_HPP_ -#include <geometry/axis/axis.hpp> #include <geometry/vector3/is_like_vector3.hpp> #include <scenario_simulator_exception/exception.hpp> @@ -23,6 +22,20 @@ namespace math { namespace geometry { +inline Eigen::Vector3d axisToEigenAxis(Axis axis) +{ + switch (axis) { + case Axis::X: + return Eigen::Vector3d::UnitX(); + case Axis::Y: + return Eigen::Vector3d::UnitY(); + case Axis::Z: + return Eigen::Vector3d::UnitZ(); + default: + THROW_SIMULATION_ERROR("Invalid axis specified."); + } +} + // Rotate a vector by a given angle around a specified axis template < typename T, std::enable_if_t<std::conjunction_v<IsLikeVector3<T>>, std::nullptr_t> = nullptr> @@ -30,15 +43,15 @@ auto rotate(T & v, const double angle, const Axis axis) { if (!std::isfinite(angle)) { THROW_SIMULATION_ERROR("The provided angle for rotation is not finite."); - } else { - const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axisToEigenAxis(axis))); - const Eigen::Vector3d eigen_vector(v.x, v.y, v.z); - const Eigen::Vector3d rotated_vector = rotation * eigen_vector; - - v.x = rotated_vector.x(); - v.y = rotated_vector.y(); - v.z = rotated_vector.z(); } + + const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axisToEigenAxis(axis))); + const Eigen::Vector3d eigen_vector(v.x, v.y, v.z); + const Eigen::Vector3d rotated_vector = rotation * eigen_vector; + + v.x = rotated_vector.x(); + v.y = rotated_vector.y(); + v.z = rotated_vector.z(); } } // namespace geometry } // namespace math diff --git a/common/math/geometry/src/axis/axis.cpp b/common/math/geometry/src/axis/axis.cpp deleted file mode 100644 index 422ddbbc36c..00000000000 --- a/common/math/geometry/src/axis/axis.cpp +++ /dev/null @@ -1,94 +0,0 @@ -// 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. - -#include <boost/geometry.hpp> -// #include <boost/geometry/geometries/point_xy.hpp> -// #include <boost/geometry/geometries/polygon.hpp> -// #include <geometry/polygon/polygon.hpp> -// #include <rclcpp/rclcpp.hpp> -#include <geometry/axis/axis.hpp> -#include <scenario_simulator_exception/exception.hpp> - -namespace math -{ -namespace geometry -{ -auto axisToEigenAxis(Axis axis) -> Eigen::Vector3d -{ - switch (axis) { - case Axis::X: - return Eigen::Vector3d::UnitX(); - case Axis::Y: - return Eigen::Vector3d::UnitY(); - case Axis::Z: - return Eigen::Vector3d::UnitZ(); - default: - THROW_SIMULATION_ERROR("Invalid axis specified."); - } -} - -auto getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) -> double -{ - if (const auto values = filterByAxis(points, axis); values.size() == 1) { - return values.front(); - } else { - return *std::max_element(values.begin(), values.end()); - } -} - -auto getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) -> double -{ - if (const auto values = filterByAxis(points, axis); values.size() == 1) { - return values.front(); - } else { - return *std::min_element(values.begin(), values.end()); - } -} - -auto filterByAxis(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) - -> std::vector<double> -{ - if (points.empty()) { - THROW_SIMULATION_ERROR( - "Invalid point list is specified, while trying to filter ", - axis == Axis::X ? "X" - : axis == Axis::Y ? "Y" - : "Z", - " axis. ", - "The point list in filterByAxis() should have at least one point to filter. " - "This message is not originally intended to be displayed, if you see it, please " - "contact the developer of traffic_simulator."); - } else { - const auto axisExtractor = - [](const Axis axis) -> std::function<double(const geometry_msgs::msg::Point &)> { - switch (axis) { - case Axis::X: - return [](const geometry_msgs::msg::Point & p) { return p.x; }; - case Axis::Y: - return [](const geometry_msgs::msg::Point & p) { return p.y; }; - case Axis::Z: - return [](const geometry_msgs::msg::Point & p) { return p.z; }; - default: - throw std::invalid_argument("Invalid axis"); - } - }; - - std::vector<double> single_axis; - std::transform( - points.begin(), points.end(), std::back_inserter(single_axis), axisExtractor(axis)); - return single_axis; - } -} -} // namespace geometry -} // namespace math diff --git a/common/math/geometry/src/polygon/polygon.cpp b/common/math/geometry/src/polygon/polygon.cpp index 06886e6299a..7a7148b40d8 100644 --- a/common/math/geometry/src/polygon/polygon.cpp +++ b/common/math/geometry/src/polygon/polygon.cpp @@ -47,5 +47,72 @@ std::vector<geometry_msgs::msg::Point> get2DConvexHull( } return polygon; } + +double getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) +{ + if (points.empty()) { + THROW_SIMULATION_ERROR( + "Invalid point list is specified, while getting max value on ", + axis == Axis::X ? "X" + : axis == Axis::Y ? "Y" + : "Z", + " axis. ", + "The point list in getMaxValue should have at least one point to get the max value from. " + "This message is not originally intended to be displayed, if you see it, please " + "contact the developer of traffic_simulator."); + } + const auto values = filterByAxis(points, axis); + if (values.size() == 1) { + return values.front(); + } + return *std::max_element(values.begin(), values.end()); +} + +double getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) +{ + if (points.empty()) { + THROW_SIMULATION_ERROR( + "Invalid point list is specified, while getting min value on ", + axis == Axis::X ? "X" + : axis == Axis::Y ? "Y" + : "Z", + " axis. ", + "The point list in getMinValue should have at least one point to get the min value from. " + "This message is not originally intended to be displayed, if you see it, please " + "contact the developer of traffic_simulator."); + } + const auto values = filterByAxis(points, axis); + if (values.size() == 1) { + return values.front(); + } + return *std::min_element(values.begin(), values.end()); +} + +std::vector<double> filterByAxis( + const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) +{ + std::vector<double> ret; + switch (axis) { + case Axis::X: { + std::transform( + points.begin(), points.end(), std::back_inserter(ret), + [](const geometry_msgs::msg::Point & point) { return point.x; }); + break; + } + case Axis::Y: { + std::transform( + points.begin(), points.end(), std::back_inserter(ret), + [](const geometry_msgs::msg::Point & point) { return point.y; }); + break; + } + case Axis::Z: { + std::transform( + points.begin(), points.end(), std::back_inserter(ret), + [](const geometry_msgs::msg::Point & point) { return point.z; }); + break; + } + } + return ret; +} } // namespace geometry } // namespace math diff --git a/common/math/geometry/test/src/polygon/test_polygon.cpp b/common/math/geometry/test/src/polygon/test_polygon.cpp index 9b4596e9913..e7278fa6566 100644 --- a/common/math/geometry/test/src/polygon/test_polygon.cpp +++ b/common/math/geometry/test/src/polygon/test_polygon.cpp @@ -14,7 +14,6 @@ #include <gtest/gtest.h> -#include <geometry/axis/axis.hpp> #include <geometry/polygon/polygon.hpp> #include <scenario_simulator_exception/exception.hpp> @@ -42,7 +41,6 @@ TEST(Polygon, filterByAxis) EXPECT_DOUBLE_EQ(values_z[2], -3.0); } -/// @todo refactor test to throw exception TEST(Polygon, filterByAxisEmptyVector) { std::vector<geometry_msgs::msg::Point> points; diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp index b9c6b9d2fb4..c514d40247e 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp @@ -18,7 +18,6 @@ #include <embree4/rtcore.h> #include <algorithm> -#include <geometry/axis/axis.hpp> #include <geometry/polygon/polygon.hpp> #include <geometry_msgs/msg/pose.hpp> #include <optional> From ab03b85f86adc39b0c563e00948353aea267db38 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 10:50:56 +0100 Subject: [PATCH 12/38] ref(geometry): remove rotate --- .../include/geometry/vector3/rotate.hpp | 59 ------------------- 1 file changed, 59 deletions(-) delete mode 100644 common/math/geometry/include/geometry/vector3/rotate.hpp diff --git a/common/math/geometry/include/geometry/vector3/rotate.hpp b/common/math/geometry/include/geometry/vector3/rotate.hpp deleted file mode 100644 index 33648f01f83..00000000000 --- a/common/math/geometry/include/geometry/vector3/rotate.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// 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__VECTOR3__ROTATE_HPP_ -#define GEOMETRY__VECTOR3__ROTATE_HPP_ - -#include <geometry/vector3/is_like_vector3.hpp> -#include <scenario_simulator_exception/exception.hpp> - -namespace math -{ -namespace geometry -{ -inline Eigen::Vector3d axisToEigenAxis(Axis axis) -{ - switch (axis) { - case Axis::X: - return Eigen::Vector3d::UnitX(); - case Axis::Y: - return Eigen::Vector3d::UnitY(); - case Axis::Z: - return Eigen::Vector3d::UnitZ(); - default: - THROW_SIMULATION_ERROR("Invalid axis specified."); - } -} - -// Rotate a vector by a given angle around a specified axis -template < - typename T, std::enable_if_t<std::conjunction_v<IsLikeVector3<T>>, std::nullptr_t> = nullptr> -auto rotate(T & v, const double angle, const Axis axis) -{ - if (!std::isfinite(angle)) { - THROW_SIMULATION_ERROR("The provided angle for rotation is not finite."); - } - - const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axisToEigenAxis(axis))); - const Eigen::Vector3d eigen_vector(v.x, v.y, v.z); - const Eigen::Vector3d rotated_vector = rotation * eigen_vector; - - v.x = rotated_vector.x(); - v.y = rotated_vector.y(); - v.z = rotated_vector.z(); -} -} // namespace geometry -} // namespace math - -#endif // GEOMETRY__VECTOR3__ROTATE_HPP_ From 0977d80a76be8771c404f9cab37c0b0618a39500 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 11:10:34 +0100 Subject: [PATCH 13/38] ref(traffic_simulator): cleanup after removal of pose::moveAlongLanelet --- .../behavior_tree_plugin/src/action_node.cpp | 6 +- .../hdmap_utils/hdmap_utils.hpp | 7 -- .../include/traffic_simulator/utils/pose.hpp | 5 -- .../src/hdmap_utils/hdmap_utils.cpp | 21 ------ .../traffic_simulator/src/utils/pose.cpp | 69 ++----------------- 5 files changed, 8 insertions(+), 100 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 4f1813538df..20fe20c6806 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -18,10 +18,8 @@ #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 <geometry/vector3/rotate.hpp> #include <memory> #include <optional> #include <rclcpp/rclcpp.hpp> @@ -540,10 +538,10 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation); pose_new.orientation = canonicalized_entity_status->getMapPose().orientation * delta_quaternion; // apply position change - const Eigen::Matrix3d rotation_matix = math::geometry::getRotationMatrix(pose_new.orientation); + const Eigen::Matrix3d rotation_matrix = math::geometry::getRotationMatrix(pose_new.orientation); const Eigen::Vector3d translation = Eigen::Vector3d(twist_new.linear.x * step_time, twist_new.linear.y * step_time, 0.0); - const Eigen::Vector3d delta_position_eigen = rotation_matix * translation; + const Eigen::Vector3d delta_position_eigen = rotation_matrix * translation; /// @todo allow: canonicalized_entity_status->getMapPose().position + delta_position_eigen geometry_msgs::msg::Vector3 delta_position; delta_position.x = delta_position_eigen.x(); 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 9b804fd8290..f88afe67e5d 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,16 +162,9 @@ 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<lanelet::Id>; - 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/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 1152807dc91..4982a2395c1 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -60,11 +60,6 @@ auto transformRelativePoseToGlobal( const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose) -> geometry_msgs::msg::Pose; -auto moveAlongLanelet( - const CanonicalizedLaneletPose & canonicalized_lanelet_pose, - const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw, - const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose; - auto moveToLaneletPose( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity, diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index de6e9c92f68..b5c91645ce9 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1098,33 +1098,12 @@ auto HdMapUtils::getNextLaneletIds( return sortAndUnique(ids); } -auto HdMapUtils::getNextLaneletOnRoute( - const lanelet::Id current_lanelet_id, const lanelet::Id destination_lanelet_id) const - -> std::optional<lanelet::Id> -{ - 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; diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index b211a2b3068..970fd83ccad 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -13,11 +13,9 @@ // limitations under the License. #include <geometry/bounding_box.hpp> -#include <geometry/quaternion/normalize.hpp> -#include <geometry/quaternion/quaternion_to_euler.hpp> +#include <geometry/vector3/norm.hpp> #include <geometry/vector3/normalize.hpp> #include <geometry/vector3/operator.hpp> -#include <geometry/vector3/rotate.hpp> #include <traffic_simulator/helper/helper.hpp> #include <traffic_simulator/utils/distance.hpp> #include <traffic_simulator/utils/pose.hpp> @@ -138,62 +136,6 @@ auto transformRelativePoseToGlobal( return ret; } -/// @todo remove adjust yaw... figure out a better solution, adjust_yaw is local.. -auto moveAlongLanelet( - const CanonicalizedLaneletPose & canonicalized_lanelet_pose, - const geometry_msgs::msg::Vector3 & desired_velocity, const auto step_time, const bool adjust_yaw, - const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose -{ - 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 remaining_lanelet_length = - hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s; - - auto pose_new = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose); - auto displacement = desired_velocity * step_time; - - /// @todo from this - these lines are just weird - // Apply pitch rotation based on the lanelet's pitch angle - const auto lanelet_pitch = - math::geometry::convertQuaternionToEulerAngle( - hdmap_utils_ptr->toMapOrientation(lanelet_pose.lanelet_id, lanelet_pose.s)) - .y; - math::geometry::rotate(displacement, lanelet_pitch, math::geometry::Axis::Y); - - // Apply yaw rotation once to avoid cumulative lateral offset errors - if (adjust_yaw) { - const auto yaw = math::geometry::convertQuaternionToEulerAngle(pose_new.orientation).z; - math::geometry::rotate(displacement, yaw, math::geometry::Axis::Z); - } - /// @todo to this - these lines are just weird - - // Check if the displacement exceeds the remaining lanelet length - if (math::geometry::norm(displacement) > remaining_lanelet_length) { - const auto excess_displacement = - displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length; - /// @todo why always first next_lanelet? (route?) - if (const auto next_lanelet_ids = hdmap_utils_ptr->getNextLaneletIds(lanelet_pose.lanelet_id); - !next_lanelet_ids.empty()) { - // transition to next lanelet - const auto s = math::geometry::norm(excess_displacement); - pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_ids[0], lanelet_pose.s); - // Apply lateral offset if transitioning to the next lanelet - /// @todo offset is not the same as y... - pose_new.position.y += canonicalized_lanelet_pose.getLaneletPose().offset; - } else { - pose_new.position += displacement; - } - } else { - pose_new.position += displacement; - } - /// @todo orientation? - return pose_new; -} - /// @todo merge moveAlongLanelet and moveToLaneletPose or separate the common part auto moveToLaneletPose( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, @@ -213,12 +155,13 @@ auto moveToLaneletPose( auto pose_new = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose); const auto displacement = desired_velocity * step_time; - // Adjust position if displacement exceeds the current lanelet length. + // adjust position if displacement exceeds the current lanelet length if (math::geometry::norm(displacement) > remaining_lanelet_length) { - const auto excess_displacement = + const auto next_lanelet_displacement = displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length; - const auto s = math::geometry::norm(excess_displacement); - pose_new.position = hdmap_utils_ptr->toMapPosition(next_lanelet_pose.lanelet_id, s); + const auto next_lanelet_s = math::geometry::norm(next_lanelet_displacement); + pose_new.position = + hdmap_utils_ptr->toMapPosition(next_lanelet_pose.lanelet_id, next_lanelet_s); // apply lateral offset if transitioning to the next lanelet /// @todo offset is not the same as y... pose_new.position.y += lanelet_pose.offset; From 5dfaf19a49747aec0db4b2426fa016fa737f6d66 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 13:17:11 +0100 Subject: [PATCH 14/38] feat(traffic_simulator): improve moveTowardsLaneletPose to calc LaneletPose and return it --- .../include/geometry/vector3/operator.hpp | 12 +++++ .../behavior_tree_plugin/src/action_node.cpp | 27 ++++++----- .../include/traffic_simulator/utils/pose.hpp | 4 +- .../src/behavior/follow_trajectory.cpp | 17 ++++--- .../traffic_simulator/src/utils/pose.cpp | 46 +++++++++++-------- 5 files changed, 67 insertions(+), 39 deletions(-) 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 <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> = diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 20fe20c6806..0cb29b25276 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/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/hypot.hpp> #include <geometry/vector3/normalize.hpp> #include <memory> #include <optional> @@ -541,25 +542,29 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const Eigen::Matrix3d rotation_matrix = math::geometry::getRotationMatrix(pose_new.orientation); const Eigen::Vector3d translation = Eigen::Vector3d(twist_new.linear.x * step_time, twist_new.linear.y * step_time, 0.0); - const Eigen::Vector3d delta_position_eigen = rotation_matrix * translation; - /// @todo allow: canonicalized_entity_status->getMapPose().position + delta_position_eigen - geometry_msgs::msg::Vector3 delta_position; - delta_position.x = delta_position_eigen.x(); - delta_position.y = delta_position_eigen.y(); - delta_position.z = delta_position_eigen.z(); + const Eigen::Vector3d delta_position = rotation_matrix * translation; pose_new.position = canonicalized_entity_status->getMapPose().position + delta_position; + // if the transition between lanelet pose: optionally overwrite position if ( const auto canonicalized_lanelet_pose = canonicalized_entity_status->getCanonicalizedLaneletPose()) { - const auto next_lanelet_pose = hdmap_utils->toLaneletPose( + const auto estimated_next_lanelet_pose = hdmap_utils->toLaneletPose( pose_new, canonicalized_entity_status->getBoundingBox(), include_crosswalk, default_matching_distance_for_lanelet_pose_calculation); - if (next_lanelet_pose) { - pose_new = traffic_simulator::pose::moveToLaneletPose( - canonicalized_lanelet_pose.value(), next_lanelet_pose.value(), twist_new.linear, step_time, - hdmap_utils); + // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same + if (estimated_next_lanelet_pose) { + const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose( + canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(), twist_new.linear, + step_time, hdmap_utils); + const auto was_position = pose_new.position; + pose_new.position = + traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils).position; + // if (hypot(was_position, pose_new.position) > 0.1) { + // THROW_SIMULATION_ERROR( + // "Position override bug by method pose::moveTowardsLaneletPose() - too much change."); + // } } } diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 4982a2395c1..04ec20ccce6 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -60,11 +60,11 @@ auto transformRelativePoseToGlobal( const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose) -> geometry_msgs::msg::Pose; -auto moveToLaneletPose( +auto moveTowardsLaneletPose( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) - -> geometry_msgs::msg::Pose; + -> LaneletPose; // Relative msg::Pose auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index bcab441bd66..a8a91f04442 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -575,20 +575,25 @@ auto makeUpdatedStatus( } }(); - // optionally overwrite pose - /// @todo is the orientation changed in moveToLaneletPose? + // if the transition between lanelet pose: optionally overwrite position if (entity_status.lanelet_pose_valid) { 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 next_lanelet_pose = hdmap_utils->toLaneletPose( + const auto estimated_next_lanelet_pose = hdmap_utils->toLaneletPose( updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance); - if (canonicalized_lanelet_pose && next_lanelet_pose) { - updated_status.pose = pose::moveToLaneletPose( - canonicalized_lanelet_pose.value(), next_lanelet_pose.value(), desired_velocity, + if (canonicalized_lanelet_pose && estimated_next_lanelet_pose) { + const auto next_lanelet_pose = pose::moveTowardsLaneletPose( + canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(), desired_velocity, step_time, hdmap_utils); + const auto was_position = updated_status.pose.position; + updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position; + // if (hypot(was_position, updated_status.pose.position) > 0.1) { + // THROW_SIMULATION_ERROR( + // "Position override bug by method pose::moveTowardsLaneletPose() - too much change."); + // } } } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 970fd83ccad..5a7c563a570 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -136,12 +136,13 @@ auto transformRelativePoseToGlobal( return ret; } -/// @todo merge moveAlongLanelet and moveToLaneletPose or separate the common part -auto moveToLaneletPose( +/// @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 LaneletPose & next_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) - -> geometry_msgs::msg::Pose + -> LaneletPose { using math::geometry::operator*; using math::geometry::operator+; @@ -149,27 +150,32 @@ auto moveToLaneletPose( using math::geometry::operator+=; const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose); + const auto yaw_relative_to_lanelet = lanelet_pose.rpy.z; + const auto longitudinal_d = (desired_velocity.x * cos(yaw_relative_to_lanelet) - + desired_velocity.y * sin(yaw_relative_to_lanelet)) * + step_time; + const auto lateral_d = (desired_velocity.x * sin(yaw_relative_to_lanelet) + + desired_velocity.y * cos(yaw_relative_to_lanelet)) * + step_time; + + LaneletPose result_lanelet_pose; const auto remaining_lanelet_length = hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s; - - auto pose_new = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose); - const auto displacement = desired_velocity * step_time; - - // adjust position if displacement exceeds the current lanelet length - if (math::geometry::norm(displacement) > remaining_lanelet_length) { - const auto next_lanelet_displacement = - displacement - math::geometry::normalize(desired_velocity) * remaining_lanelet_length; - const auto next_lanelet_s = math::geometry::norm(next_lanelet_displacement); - pose_new.position = - hdmap_utils_ptr->toMapPosition(next_lanelet_pose.lanelet_id, next_lanelet_s); - // apply lateral offset if transitioning to the next lanelet - /// @todo offset is not the same as y... - pose_new.position.y += lanelet_pose.offset; + 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; + result_lanelet_pose.offset = lanelet_pose.offset + lateral_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; + result_lanelet_pose.offset = lanelet_pose.offset + lateral_d; } else { - pose_new.position += displacement; + THROW_SIMULATION_ERROR("Next lanelet is too short."); } - /// @todo orientation? - return pose_new; + 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) From c1df2c549833468978afed173294420cf5bed672 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 13:19:46 +0100 Subject: [PATCH 15/38] ref(traffic_simulator): remove irrelevant toMapPosition --- .../include/traffic_simulator/hdmap_utils/hdmap_utils.hpp | 3 --- .../traffic_simulator/src/hdmap_utils/hdmap_utils.cpp | 6 ------ 2 files changed, 9 deletions(-) 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 f88afe67e5d..9be623a2cab 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,9 +162,6 @@ class HdMapUtils auto getAltitude(const traffic_simulator_msgs::msg::LaneletPose &) const -> double; - auto toMapPosition(const lanelet::Id lanelet_id, const double s) const - -> geometry_msgs::msg::Point; - auto getLaneChangeTrajectory( const geometry_msgs::msg::Pose & from, const traffic_simulator::lane_change::Parameter & lane_change_parameter, diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index b5c91645ce9..eab04aaf2f2 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1098,12 +1098,6 @@ auto HdMapUtils::getNextLaneletIds( return sortAndUnique(ids); } -auto HdMapUtils::toMapPosition(const lanelet::Id lanelet_id, const double s) const - -> geometry_msgs::msg::Point -{ - return getCenterPointsSpline(lanelet_id)->getPoint(s); -} - auto HdMapUtils::getTrafficLightIds() const -> lanelet::Ids { using namespace lanelet::utils::query; From 14098e974f96c91c1eeaa162f3e867d72c5b5d22 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 14:16:49 +0100 Subject: [PATCH 16/38] fix(traffic_simulator): fix moveTowardsLaneletPose --- .../behavior_tree_plugin/src/action_node.cpp | 8 +++---- .../src/behavior/follow_trajectory.cpp | 8 +++---- .../traffic_simulator/src/utils/pose.cpp | 23 +++++++++++-------- 3 files changed, 22 insertions(+), 17 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 0cb29b25276..4abb5f647dc 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -561,10 +561,10 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const auto was_position = pose_new.position; pose_new.position = traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils).position; - // if (hypot(was_position, pose_new.position) > 0.1) { - // THROW_SIMULATION_ERROR( - // "Position override bug by method pose::moveTowardsLaneletPose() - too much change."); - // } + if (math::geometry::hypot(was_position, pose_new.position) > 0.1) { + THROW_SIMULATION_ERROR( + "Position override bug by method pose::moveTowardsLaneletPose() - too much change."); + } } } diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index a8a91f04442..a72ff2e3a95 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -590,10 +590,10 @@ auto makeUpdatedStatus( step_time, hdmap_utils); const auto was_position = updated_status.pose.position; updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position; - // if (hypot(was_position, updated_status.pose.position) > 0.1) { - // THROW_SIMULATION_ERROR( - // "Position override bug by method pose::moveTowardsLaneletPose() - too much change."); - // } + if (hypot(was_position, updated_status.pose.position) > 0.1) { + THROW_SIMULATION_ERROR( + "Position override bug by method pose::moveTowardsLaneletPose() - too much change."); + } } } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 5a7c563a570..302a6a00e1e 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -150,13 +150,19 @@ auto moveTowardsLaneletPose( using math::geometry::operator+=; const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose); - const auto yaw_relative_to_lanelet = lanelet_pose.rpy.z; - const auto longitudinal_d = (desired_velocity.x * cos(yaw_relative_to_lanelet) - - desired_velocity.y * sin(yaw_relative_to_lanelet)) * - step_time; - const auto lateral_d = (desired_velocity.x * sin(yaw_relative_to_lanelet) + - desired_velocity.y * cos(yaw_relative_to_lanelet)) * - step_time; + + // 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; + // determine the displacement in the 2D lanelet coordinate system + const Eigen::Vector2d displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) * + Eigen::Vector2d(local_velocity.x(), local_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 = @@ -165,15 +171,14 @@ auto moveTowardsLaneletPose( if (longitudinal_d < remaining_lanelet_length) { result_lanelet_pose.lanelet_id = lanelet_pose.lanelet_id; result_lanelet_pose.s = lanelet_pose.s + longitudinal_d; - result_lanelet_pose.offset = lanelet_pose.offset + lateral_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; - result_lanelet_pose.offset = lanelet_pose.offset + lateral_d; } else { THROW_SIMULATION_ERROR("Next lanelet is too short."); } + result_lanelet_pose.offset = lanelet_pose.offset + lateral_d; result_lanelet_pose.rpy = lanelet_pose.rpy; return result_lanelet_pose; } From 311f82a25de3aed1eb4328aa974345c2e561a2fc Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 14:31:35 +0100 Subject: [PATCH 17/38] ref(geometry): remove confusing operator*(quat,vec) --- .../include/geometry/quaternion/operator.hpp | 28 ------------------- 1 file changed, 28 deletions(-) diff --git a/common/math/geometry/include/geometry/quaternion/operator.hpp b/common/math/geometry/include/geometry/quaternion/operator.hpp index 4839a282333..848e1d8332c 100644 --- a/common/math/geometry/include/geometry/quaternion/operator.hpp +++ b/common/math/geometry/include/geometry/quaternion/operator.hpp @@ -65,34 +65,6 @@ 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 & rotation, const U & vector) -{ - T vector_as_quaternion; - vector_as_quaternion.x = vector.x; - vector_as_quaternion.y = vector.y; - vector_as_quaternion.z = vector.z; - vector_as_quaternion.w = 0.0; - - T inverse_rotation = rotation; - inverse_rotation.x = -rotation.x; - inverse_rotation.y = -rotation.y; - inverse_rotation.z = -rotation.z; - inverse_rotation.w = rotation.w; - - T rotated_quaternion = rotation * vector_as_quaternion * inverse_rotation; - - U rotated_vector; - rotated_vector.x = vector_as_quaternion.x; - rotated_vector.y = vector_as_quaternion.y; - rotated_vector.z = vector_as_quaternion.z; - - return rotated_vector; -} - template < typename T, typename U, std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>, IsLikeQuaternion<U>>, std::nullptr_t> = From e3df32007790fbf1e0e30eb14cafe5cb44353caf Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 15:05:43 +0100 Subject: [PATCH 18/38] ref(behavior_tree_plugin): refactor ActionNode::calculateUpdatedEntityStatusInWorldFrame --- .../include/geometry/quaternion/operator.hpp | 1 - .../behavior_tree_plugin/src/action_node.cpp | 86 ++++++++++--------- 2 files changed, 47 insertions(+), 40 deletions(-) diff --git a/common/math/geometry/include/geometry/quaternion/operator.hpp b/common/math/geometry/include/geometry/quaternion/operator.hpp index 848e1d8332c..624dd686852 100644 --- a/common/math/geometry/include/geometry/quaternion/operator.hpp +++ b/common/math/geometry/include/geometry/quaternion/operator.hpp @@ -16,7 +16,6 @@ #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 diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 4abb5f647dc..0ccdbc78622 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -522,51 +522,59 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( (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 step_time, + const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) + -> geometry_msgs::msg::Pose { + geometry_msgs::msg::Pose updated_pose; + + // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s) + geometry_msgs::msg::Vector3 delta_rotation; + delta_rotation.z = desired_twist.angular.z * step_time; + const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation); + updated_pose.orientation = status->getMapPose().orientation * delta_quaternion; + + // apply position change + const Eigen::Matrix3d rotation_matrix = + math::geometry::getRotationMatrix(updated_pose.orientation); + const Eigen::Vector3d translation = Eigen::Vector3d( + desired_twist.linear.x * step_time, desired_twist.linear.y * step_time, + desired_twist.linear.z * step_time); + const Eigen::Vector3d delta_position = rotation_matrix * translation; + updated_pose.position = status->getMapPose().position + delta_position; + + // if it is the transition between lanelet pose: optionally overwrite position to improve precision + if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) { + const auto estimated_next_lanelet_pose = hdmap_utils_ptr->toLaneletPose( + updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance); + + // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same + if (estimated_next_lanelet_pose) { + const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose( + canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(), + desired_twist.linear, step_time, 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; - // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s) - geometry_msgs::msg::Vector3 delta_rotation; - delta_rotation.z = twist_new.angular.z * step_time; - const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation); - pose_new.orientation = canonicalized_entity_status->getMapPose().orientation * delta_quaternion; - // apply position change - const Eigen::Matrix3d rotation_matrix = math::geometry::getRotationMatrix(pose_new.orientation); - const Eigen::Vector3d translation = - Eigen::Vector3d(twist_new.linear.x * step_time, twist_new.linear.y * step_time, 0.0); - const Eigen::Vector3d delta_position = rotation_matrix * translation; - pose_new.position = canonicalized_entity_status->getMapPose().position + delta_position; - - // if the transition between lanelet pose: optionally overwrite position - if ( - const auto canonicalized_lanelet_pose = - canonicalized_entity_status->getCanonicalizedLaneletPose()) { - const auto estimated_next_lanelet_pose = hdmap_utils->toLaneletPose( - pose_new, canonicalized_entity_status->getBoundingBox(), include_crosswalk, - default_matching_distance_for_lanelet_pose_calculation); - - // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same - if (estimated_next_lanelet_pose) { - const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose( - canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(), twist_new.linear, - step_time, hdmap_utils); - const auto was_position = pose_new.position; - pose_new.position = - traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils).position; - if (math::geometry::hypot(was_position, pose_new.position) > 0.1) { - THROW_SIMULATION_ERROR( - "Position override bug by method pose::moveTowardsLaneletPose() - too much change."); - } - } - } + 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); From 4efd379f5ae52cbba916e0c7325ec1f99d7cd7b0 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 15:13:08 +0100 Subject: [PATCH 19/38] ref(behavior_tree_plugin, traffic_simulator): improve comments --- simulation/behavior_tree_plugin/src/action_node.cpp | 5 ++--- .../traffic_simulator/src/behavior/follow_trajectory.cpp | 7 +------ 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 0ccdbc78622..5e7c404c270 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -19,8 +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/hypot.hpp> -#include <geometry/vector3/normalize.hpp> +#include <geometry/vector3/operator.hpp> #include <memory> #include <optional> #include <rclcpp/rclcpp.hpp> @@ -547,7 +546,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const Eigen::Vector3d delta_position = rotation_matrix * translation; updated_pose.position = status->getMapPose().position + delta_position; - // if it is the transition between lanelet pose: optionally overwrite position to improve precision + // 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_lanelet_pose = hdmap_utils_ptr->toLaneletPose( updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance); diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index a72ff2e3a95..029a07007dc 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -575,7 +575,7 @@ auto makeUpdatedStatus( } }(); - // if the transition between lanelet pose: optionally overwrite position + // if it is the transition between lanelet pose: overwrite position to improve precision if (entity_status.lanelet_pose_valid) { const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( entity_status.pose, entity_status.bounding_box, {entity_status.lanelet_pose.lanelet_id}, @@ -588,12 +588,7 @@ auto makeUpdatedStatus( const auto next_lanelet_pose = pose::moveTowardsLaneletPose( canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(), desired_velocity, step_time, hdmap_utils); - const auto was_position = updated_status.pose.position; updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position; - if (hypot(was_position, updated_status.pose.position) > 0.1) { - THROW_SIMULATION_ERROR( - "Position override bug by method pose::moveTowardsLaneletPose() - too much change."); - } } } From c59228c5c30fb7cc44048dd9df1be0d83a6dd57a Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Thu, 19 Dec 2024 16:06:59 +0100 Subject: [PATCH 20/38] fix(traffis_simulator): use next canonicalized lanelet pose in pose::moveTowardsLaneletPose --- .../behavior_tree_plugin/src/action_node.cpp | 10 ++++++---- .../include/traffic_simulator/utils/pose.hpp | 6 +++--- .../src/behavior/follow_trajectory.cpp | 19 ++++++++++++------- .../traffic_simulator/src/utils/pose.cpp | 16 ++++++++++------ 4 files changed, 31 insertions(+), 20 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 5e7c404c270..9928346b1ae 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -548,13 +548,15 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( // 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_lanelet_pose = hdmap_utils_ptr->toLaneletPose( - updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance); + 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_lanelet_pose) { + if (estimated_next_canonicalized_lanelet_pose) { const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose( - canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(), + canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(), desired_twist.linear, step_time, hdmap_utils_ptr); updated_pose.position = traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 04ec20ccce6..5d6cceafb4a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -62,9 +62,9 @@ auto transformRelativePoseToGlobal( auto moveTowardsLaneletPose( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, - const LaneletPose & target_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity, - const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) - -> LaneletPose; + const CanonicalizedLaneletPose & next_canonicalized_lanelet_pose, + const geometry_msgs::msg::Vector3 & desired_velocity, 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) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 029a07007dc..d95e21889ae 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -558,6 +558,8 @@ auto makeUpdatedStatus( */ auto updated_status = entity_status; + updated_status.lanelet_pose_valid = false; + updated_status.pose.position += desired_velocity * step_time; updated_status.pose.orientation = [&]() { @@ -581,14 +583,19 @@ auto makeUpdatedStatus( entity_status.pose, entity_status.bounding_box, {entity_status.lanelet_pose.lanelet_id}, include_crosswalk, matching_distance, hdmap_utils); - const auto estimated_next_lanelet_pose = hdmap_utils->toLaneletPose( - updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance); + 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_lanelet_pose) { + if (canonicalized_lanelet_pose && estimated_next_canonicalized_lanelet_pose) { const auto next_lanelet_pose = pose::moveTowardsLaneletPose( - canonicalized_lanelet_pose.value(), estimated_next_lanelet_pose.value(), desired_velocity, - step_time, hdmap_utils); + canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(), + desired_velocity, step_time, hdmap_utils); updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position; + /// @todo uncomment to test it! + // updated_status.lanelet_pose = next_lanelet_pose + // updated_status.lanelet_pose_valid = true; } } @@ -613,8 +620,6 @@ auto makeUpdatedStatus( updated_status.time = entity_status.time + step_time; - updated_status.lanelet_pose_valid = false; - return updated_status; } } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 302a6a00e1e..56df046f604 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -140,9 +140,9 @@ auto transformRelativePoseToGlobal( // the orientation remains the same as in next_lanelet_pose auto moveTowardsLaneletPose( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, - const LaneletPose & next_lanelet_pose, const geometry_msgs::msg::Vector3 & desired_velocity, - const double step_time, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) - -> LaneletPose + const CanonicalizedLaneletPose & next_canonicalized_lanelet_pose, + const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time, + const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> LaneletPose { using math::geometry::operator*; using math::geometry::operator+; @@ -150,6 +150,7 @@ auto moveTowardsLaneletPose( 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); // transform desired (global) velocity to local velocity const auto orientation = @@ -168,17 +169,20 @@ auto moveTowardsLaneletPose( 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) { + if (longitudinal_d <= remaining_lanelet_length) { result_lanelet_pose.lanelet_id = lanelet_pose.lanelet_id; result_lanelet_pose.s = lanelet_pose.s + longitudinal_d; + result_lanelet_pose.offset = lanelet_pose.offset + lateral_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; + result_lanelet_pose.offset = next_lanelet_pose.offset + lateral_d; } else { - THROW_SIMULATION_ERROR("Next lanelet is too short."); + THROW_SIMULATION_ERROR( + "Next lanelet is too short: lanelet_id==", next_lanelet_pose.lanelet_id, " is shroter 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; } From 4b27b38d7ff729c3583f59785393f5037fdd188e Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Fri, 20 Dec 2024 11:28:52 +0100 Subject: [PATCH 21/38] fix(traffic_simulator) Fix an issue with negative longitudinal displacement --- .../src/behavior/follow_trajectory.cpp | 3 -- .../traffic_simulator/src/utils/pose.cpp | 33 +++++++++++++++++-- 2 files changed, 30 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index d95e21889ae..38cd007f203 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -593,9 +593,6 @@ auto makeUpdatedStatus( canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(), desired_velocity, step_time, hdmap_utils); updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position; - /// @todo uncomment to test it! - // updated_status.lanelet_pose = next_lanelet_pose - // updated_status.lanelet_pose_valid = true; } } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 56df046f604..9f3ee0e17e3 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -13,6 +13,8 @@ // 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> @@ -152,14 +154,39 @@ auto moveTowardsLaneletPose( const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose); const auto next_lanelet_pose = static_cast<LaneletPose>(next_canonicalized_lanelet_pose); - // transform desired (global) velocity to local velocity + /* + * When the yaw of the lanelet is outside the range -90° to +90°, + * transforming the global velocity using this orientation results + * in a negative X component of the local velocity. This causes a + * negative longitudinal displacement, making the vehicle appear + * to move backward even when the intended motion is forward. + * + * To address this issue, we adjust the yaw of the lanelet by ±180° + * if it falls outside the range -90° to +90°. This correction ensures + * that the local velocity's X component remains positive, resulting + * in the vehicle moving forward as intended. + */ const auto orientation = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose).orientation; + auto orientation_rpy = math::geometry::convertQuaternionToEulerAngle(orientation); + + if (std::abs(orientation_rpy.z) > M_PI / 2) { + if (orientation_rpy.z > 0) { + orientation_rpy.z -= M_PI; + } else { + orientation_rpy.z += M_PI; + } + } + + // transform desired (global) velocity to local velocity + const auto corrected_orientation = math::geometry::convertEulerAngleToQuaternion(orientation_rpy); 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::Quaterniond quaternion( + corrected_orientation.w, corrected_orientation.x, corrected_orientation.y, + corrected_orientation.z); const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity; // determine the displacement in the 2D lanelet coordinate system - const Eigen::Vector2d displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) * + const Eigen::Vector2d displacement = Eigen::Rotation2Dd(orientation_rpy.z) * Eigen::Vector2d(local_velocity.x(), local_velocity.y()) * step_time; const auto longitudinal_d = displacement.x(); From 32a88386af39c52c7e32bee48d6628f3fd6a5ff9 Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Fri, 20 Dec 2024 11:32:17 +0100 Subject: [PATCH 22/38] refactor(traffic_simulator) fix spell check issue --- simulation/traffic_simulator/src/utils/pose.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 9f3ee0e17e3..979cefedb2f 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -207,7 +207,7 @@ auto moveTowardsLaneletPose( result_lanelet_pose.offset = next_lanelet_pose.offset + lateral_d; } else { THROW_SIMULATION_ERROR( - "Next lanelet is too short: lanelet_id==", next_lanelet_pose.lanelet_id, " is shroter than ", + "Next lanelet is too short: lanelet_id==", next_lanelet_pose.lanelet_id, " is shorter than ", next_lanelet_longitudinal_d); } result_lanelet_pose.rpy = lanelet_pose.rpy; From 788259ccacb017d5d5ca0ff389104fd7c8fa468f Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Fri, 20 Dec 2024 16:04:27 +0100 Subject: [PATCH 23/38] tmp(behavior_tree_plugin, traffic_simulator): moveTowardsLaneletPose debug --- .../behavior_tree_plugin/src/action_node.cpp | 28 +++++++-- .../follow_polyline_trajectory_action.cpp | 5 +- .../include/traffic_simulator/utils/pose.hpp | 5 +- .../src/behavior/follow_trajectory.cpp | 9 ++- .../src/data_type/lanelet_pose.cpp | 2 +- .../traffic_simulator/src/utils/pose.cpp | 58 ++++++++----------- 6 files changed, 61 insertions(+), 46 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 9928346b1ae..2f6beac294b 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -530,7 +530,6 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose { geometry_msgs::msg::Pose updated_pose; - // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s) geometry_msgs::msg::Vector3 delta_rotation; delta_rotation.z = desired_twist.angular.z * step_time; @@ -538,6 +537,8 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( 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 Eigen::Vector3d translation = Eigen::Vector3d( @@ -546,22 +547,39 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const Eigen::Vector3d delta_position = rotation_matrix * translation; updated_pose.position = status->getMapPose().position + delta_position; + std::cout << " 1 " << std::endl; // 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); - + const auto lanelet_pose = + static_cast<traffic_simulator::LaneletPose>(canonicalized_lanelet_pose.value()); + const auto pose = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose.value()); + std::cout << " 2 " << std::endl; + std::cout << " -- lanelet pose: id:" << lanelet_pose.lanelet_id << " s:" << lanelet_pose.s + << " offset:" << lanelet_pose.offset << " yaw: " << lanelet_pose.rpy.z << std::endl; + std::cout << " -- orientation: " + << math::geometry::convertQuaternionToEulerAngle(pose.orientation).z << std::endl; + std::cout << " -- d_velocity: x:" << desired_twist.linear.x << " y:" << desired_twist.linear.y + << " z:" << desired_twist.linear.z << std::endl; // 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, step_time, hdmap_utils_ptr); - updated_pose.position = - traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position; + desired_twist.linear, false, step_time, hdmap_utils_ptr); + std::cout << " 3 " << std::endl; + if ( + const auto next_canonicalized_lanelet_pose = + traffic_simulator::pose::canonicalize(next_lanelet_pose, hdmap_utils_ptr)) { + std::cout << " 4 " << std::endl; + updated_pose.position = + static_cast<geometry_msgs::msg::Pose>(next_canonicalized_lanelet_pose.value()).position; + } } } + std::cout << " ~~~~~ " << std::endl; return updated_pose; }; 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..9ae8d6e5889 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,12 +63,13 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus return canonicalized_entity_status->getTwist().linear.x; } }; - + std::cout << "FollowPolylineTrajectoryAction" << std::endl; if (getBlackBoardValues(); request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or not getInput<decltype(target_speed)>("target_speed", target_speed) or not polyline_trajectory) { + std::cout << "~~FollowPolylineTrajectoryAction" << std::endl; return BT::NodeStatus::FAILURE; } else if (std::isnan(canonicalized_entity_status->getTime())) { THROW_SIMULATION_ERROR( @@ -82,8 +83,10 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); setOutput("obstacle", calculateObstacle(calculateWaypoints())); + std::cout << "~~FollowPolylineTrajectoryAction" << std::endl; return BT::NodeStatus::RUNNING; } else { + std::cout << "~~FollowPolylineTrajectoryAction" << std::endl; return BT::NodeStatus::SUCCESS; } } diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 5d6cceafb4a..93b13227fd2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -63,8 +63,9 @@ auto transformRelativePoseToGlobal( auto moveTowardsLaneletPose( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const CanonicalizedLaneletPose & next_canonicalized_lanelet_pose, - const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time, - const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> LaneletPose; + 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) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 38cd007f203..37807afb9d1 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -591,8 +591,13 @@ auto makeUpdatedStatus( 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, step_time, hdmap_utils); - updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position; + desired_velocity, true, step_time, hdmap_utils); + if ( + const auto next_canonicalized_lanelet_pose = + pose::canonicalize(next_lanelet_pose, hdmap_utils)) { + updated_status.pose.position = + static_cast<geometry_msgs::msg::Pose>(next_canonicalized_lanelet_pose.value()).position; + } } } diff --git a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp index b4dfb4dc0d8..39b24b6c738 100644 --- a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp +++ b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp @@ -154,7 +154,7 @@ auto CanonicalizedLaneletPose::adjustOrientationAndOzPosition( .y(lanelet_rpy.y) .z(entity_rpy.z)); lanelet_pose_.rpy = - convertQuaternionToEulerAngle(getRotation(lanelet_quaternion, map_pose_.orientation)); + convertQuaternionToEulerAngle(getRotation(map_pose_.orientation, lanelet_quaternion)); } } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 979cefedb2f..69e6bada0c0 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -143,8 +143,9 @@ auto transformRelativePoseToGlobal( auto moveTowardsLaneletPose( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const CanonicalizedLaneletPose & next_canonicalized_lanelet_pose, - const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time, - const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> LaneletPose + 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+; @@ -154,41 +155,28 @@ auto moveTowardsLaneletPose( const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose); const auto next_lanelet_pose = static_cast<LaneletPose>(next_canonicalized_lanelet_pose); - /* - * When the yaw of the lanelet is outside the range -90° to +90°, - * transforming the global velocity using this orientation results - * in a negative X component of the local velocity. This causes a - * negative longitudinal displacement, making the vehicle appear - * to move backward even when the intended motion is forward. - * - * To address this issue, we adjust the yaw of the lanelet by ±180° - * if it falls outside the range -90° to +90°. This correction ensures - * that the local velocity's X component remains positive, resulting - * in the vehicle moving forward as intended. - */ + // transform desired (global) velocity to local velocity const auto orientation = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose).orientation; - auto orientation_rpy = math::geometry::convertQuaternionToEulerAngle(orientation); - if (std::abs(orientation_rpy.z) > M_PI / 2) { - if (orientation_rpy.z > 0) { - orientation_rpy.z -= M_PI; - } else { - orientation_rpy.z += M_PI; - } - } + // const auto fi = lanelet_pose.rpy.z; + // const auto orientation_rpy = math::geometry::convertQuaternionToEulerAngle(orientation); + // std::cout << "-- lanelet_pose.rpy.z: " << lanelet_pose.rpy.z + // << " | orientation_rpy.z: " << orientation_rpy.z << " | fi: " << fi << std::endl; - // transform desired (global) velocity to local velocity - const auto corrected_orientation = math::geometry::convertEulerAngleToQuaternion(orientation_rpy); - const Eigen::Vector3d global_velocity(desired_velocity.x, desired_velocity.y, desired_velocity.z); - const Eigen::Quaterniond quaternion( - corrected_orientation.w, corrected_orientation.x, corrected_orientation.y, - corrected_orientation.z); - const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity; - // determine the displacement in the 2D lanelet coordinate system - const Eigen::Vector2d displacement = Eigen::Rotation2Dd(orientation_rpy.z) * - Eigen::Vector2d(local_velocity.x(), local_velocity.y()) * - step_time; + Eigen::Vector2d displacement; + if (desired_velocity_is_global) { + 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; + // determine the displacement in the 2D lanelet coordinate system + 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(); @@ -199,12 +187,12 @@ auto moveTowardsLaneletPose( if (longitudinal_d <= remaining_lanelet_length) { result_lanelet_pose.lanelet_id = lanelet_pose.lanelet_id; result_lanelet_pose.s = lanelet_pose.s + longitudinal_d; - result_lanelet_pose.offset = lanelet_pose.offset + lateral_d; + result_lanelet_pose.offset = lanelet_pose.offset; // + lateral_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; - result_lanelet_pose.offset = next_lanelet_pose.offset + lateral_d; + result_lanelet_pose.offset = lanelet_pose.offset; // + lateral_d; } else { THROW_SIMULATION_ERROR( "Next lanelet is too short: lanelet_id==", next_lanelet_pose.lanelet_id, " is shorter than ", From b2074d48c9e559cb37a558d2fcf0292f355bff8e Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Fri, 20 Dec 2024 16:10:37 +0100 Subject: [PATCH 24/38] fix(behavior_tree_plugin, traffic_simulator): fix moveTowardsLaneletPose for WalkStraightAction, tidy up --- .../behavior_tree_plugin/src/action_node.cpp | 28 +++++-------------- .../follow_polyline_trajectory_action.cpp | 3 -- .../src/behavior/follow_trajectory.cpp | 11 +++----- .../traffic_simulator/src/utils/pose.cpp | 15 +++------- 4 files changed, 15 insertions(+), 42 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 2f6beac294b..09d3a588c5e 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -516,6 +516,8 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( 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); @@ -530,6 +532,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> geometry_msgs::msg::Pose { geometry_msgs::msg::Pose updated_pose; + // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s) geometry_msgs::msg::Vector3 delta_rotation; delta_rotation.z = desired_twist.angular.z * step_time; @@ -547,39 +550,22 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const Eigen::Vector3d delta_position = rotation_matrix * translation; updated_pose.position = status->getMapPose().position + delta_position; - std::cout << " 1 " << std::endl; // 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); - const auto lanelet_pose = - static_cast<traffic_simulator::LaneletPose>(canonicalized_lanelet_pose.value()); - const auto pose = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose.value()); - std::cout << " 2 " << std::endl; - std::cout << " -- lanelet pose: id:" << lanelet_pose.lanelet_id << " s:" << lanelet_pose.s - << " offset:" << lanelet_pose.offset << " yaw: " << lanelet_pose.rpy.z << std::endl; - std::cout << " -- orientation: " - << math::geometry::convertQuaternionToEulerAngle(pose.orientation).z << std::endl; - std::cout << " -- d_velocity: x:" << desired_twist.linear.x << " y:" << desired_twist.linear.y - << " z:" << desired_twist.linear.z << std::endl; + // 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, false, step_time, hdmap_utils_ptr); - std::cout << " 3 " << std::endl; - if ( - const auto next_canonicalized_lanelet_pose = - traffic_simulator::pose::canonicalize(next_lanelet_pose, hdmap_utils_ptr)) { - std::cout << " 4 " << std::endl; - updated_pose.position = - static_cast<geometry_msgs::msg::Pose>(next_canonicalized_lanelet_pose.value()).position; - } + desired_twist.linear, desired_velocity_is_global, step_time, hdmap_utils_ptr); + updated_pose.position = + traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position; } } - std::cout << " ~~~~~ " << std::endl; return updated_pose; }; 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 9ae8d6e5889..07b48bae9c3 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; } }; - std::cout << "FollowPolylineTrajectoryAction" << std::endl; if (getBlackBoardValues(); request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or @@ -83,10 +82,8 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); setOutput("obstacle", calculateObstacle(calculateWaypoints())); - std::cout << "~~FollowPolylineTrajectoryAction" << std::endl; return BT::NodeStatus::RUNNING; } else { - std::cout << "~~FollowPolylineTrajectoryAction" << std::endl; return BT::NodeStatus::SUCCESS; } } diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 37807afb9d1..f0da884b3b5 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -579,6 +579,8 @@ 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); @@ -591,13 +593,8 @@ auto makeUpdatedStatus( 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, true, step_time, hdmap_utils); - if ( - const auto next_canonicalized_lanelet_pose = - pose::canonicalize(next_lanelet_pose, hdmap_utils)) { - updated_status.pose.position = - static_cast<geometry_msgs::msg::Pose>(next_canonicalized_lanelet_pose.value()).position; - } + desired_velocity, desired_velocity_is_global, step_time, hdmap_utils); + updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position; } } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 69e6bada0c0..65609776db9 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -155,17 +155,11 @@ auto moveTowardsLaneletPose( const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose); const auto next_lanelet_pose = static_cast<LaneletPose>(next_canonicalized_lanelet_pose); - // transform desired (global) velocity to local velocity - const auto orientation = - static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose).orientation; - - // const auto fi = lanelet_pose.rpy.z; - // const auto orientation_rpy = math::geometry::convertQuaternionToEulerAngle(orientation); - // std::cout << "-- lanelet_pose.rpy.z: " << lanelet_pose.rpy.z - // << " | orientation_rpy.z: " << orientation_rpy.z << " | fi: " << fi << std::endl; - 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); @@ -187,17 +181,16 @@ auto moveTowardsLaneletPose( if (longitudinal_d <= remaining_lanelet_length) { result_lanelet_pose.lanelet_id = lanelet_pose.lanelet_id; result_lanelet_pose.s = lanelet_pose.s + longitudinal_d; - result_lanelet_pose.offset = lanelet_pose.offset; // + lateral_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; - result_lanelet_pose.offset = lanelet_pose.offset; // + lateral_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; } From ad6c16b9d47f74506c644832f343856a3bd66d3c Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Fri, 20 Dec 2024 16:14:48 +0100 Subject: [PATCH 25/38] ref(traffic_simulator): tidy up after moveTowardsLaneletPose development --- .../follow_polyline_trajectory_action.cpp | 1 - .../traffic_simulator/src/behavior/follow_trajectory.cpp | 4 ++-- simulation/traffic_simulator/src/utils/pose.cpp | 2 +- 3 files changed, 3 insertions(+), 4 deletions(-) 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 07b48bae9c3..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 @@ -68,7 +68,6 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or not getInput<decltype(target_speed)>("target_speed", target_speed) or not polyline_trajectory) { - std::cout << "~~FollowPolylineTrajectoryAction" << std::endl; return BT::NodeStatus::FAILURE; } else if (std::isnan(canonicalized_entity_status->getTime())) { THROW_SIMULATION_ERROR( diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index f0da884b3b5..ef9cbd8ea18 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -558,8 +558,6 @@ auto makeUpdatedStatus( */ auto updated_status = entity_status; - updated_status.lanelet_pose_valid = false; - updated_status.pose.position += desired_velocity * step_time; updated_status.pose.orientation = [&]() { @@ -619,6 +617,8 @@ auto makeUpdatedStatus( updated_status.time = entity_status.time + step_time; + updated_status.lanelet_pose_valid = false; + return updated_status; } } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 65609776db9..a1b55266d3e 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -155,6 +155,7 @@ auto moveTowardsLaneletPose( 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 @@ -164,7 +165,6 @@ auto moveTowardsLaneletPose( 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; - // determine the displacement in the 2D lanelet coordinate system displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) * Eigen::Vector2d(local_velocity.x(), local_velocity.y()) * step_time; } else { From 74bee395dd110f601708d07c86e43f12ab224aed Mon Sep 17 00:00:00 2001 From: Dawid Moszynski <dawid.moszynski@robotec.ai> Date: Fri, 20 Dec 2024 16:19:49 +0100 Subject: [PATCH 26/38] fix(traffic_simulator): revert adjustOrientationAndOzPosition change (lanelet rpy) --- simulation/traffic_simulator/src/data_type/lanelet_pose.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp index 39b24b6c738..b4dfb4dc0d8 100644 --- a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp +++ b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp @@ -154,7 +154,7 @@ auto CanonicalizedLaneletPose::adjustOrientationAndOzPosition( .y(lanelet_rpy.y) .z(entity_rpy.z)); lanelet_pose_.rpy = - convertQuaternionToEulerAngle(getRotation(map_pose_.orientation, lanelet_quaternion)); + convertQuaternionToEulerAngle(getRotation(lanelet_quaternion, map_pose_.orientation)); } } From c3e604fa3587e8369d72c7c74a0fed59796eb0c2 Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Fri, 20 Dec 2024 18:23:56 +0100 Subject: [PATCH 27/38] fix(behavior_tree_plugin) fix sonarquebe issues --- .../behavior_tree_plugin/src/action_node.cpp | 81 +++++++++---------- 1 file changed, 40 insertions(+), 41 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 09d3a588c5e..438f5b31620 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -528,46 +528,45 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const auto buildUpdatedPose = [&include_crosswalk, &matching_distance]( const std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> & status, - const geometry_msgs::msg::Twist & desired_twist, const double step_time, - const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) - -> geometry_msgs::msg::Pose { - geometry_msgs::msg::Pose updated_pose; - - // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s) - geometry_msgs::msg::Vector3 delta_rotation; - delta_rotation.z = desired_twist.angular.z * step_time; - 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 Eigen::Vector3d translation = Eigen::Vector3d( - desired_twist.linear.x * step_time, desired_twist.linear.y * step_time, - desired_twist.linear.z * step_time); - 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, step_time, hdmap_utils_ptr); - updated_pose.position = - traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position; + 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; - }; + return updated_pose; + }; const auto speed_planner = traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( @@ -576,8 +575,8 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( target_speed, constraints, canonicalized_entity_status->getTwist(), canonicalized_entity_status->getAccel()); 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 & 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); From e884623c46134588545d8639a03df2bd75d31afe Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Thu, 23 Jan 2025 11:29:00 +0100 Subject: [PATCH 28/38] fix(traffic_simulator): Fix lanelet slope inaccuracies - Adjusted entity position when transitioning between lanelets to reduce slope errors --- .../behavior_tree_plugin/src/action_node.cpp | 18 ++-- .../include/traffic_simulator/utils/pose.hpp | 11 ++- .../src/behavior/follow_trajectory.cpp | 18 ++-- .../traffic_simulator/src/utils/pose.cpp | 83 ++++++++----------- 4 files changed, 57 insertions(+), 73 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 438f5b31620..17e487539ff 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -540,7 +540,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( // apply position change /// @todo first determine global desired_velocity, calculate position change using it - // then pass the same global desired_velocity to moveTowardsLaneletPose() + // then pass the same global desired_velocity to adjustPoseForLaneletTransition() const Eigen::Matrix3d rotation_matrix = math::geometry::getRotationMatrix(updated_pose.orientation); const auto translation = Eigen::Vector3d( @@ -549,20 +549,22 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( 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 it is the transition between lanelets: overwrite position to improve precision 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; + const auto next_lanelet_id = static_cast<traffic_simulator::LaneletPose>( + estimated_next_canonicalized_lanelet_pose.value()) + .lanelet_id; + auto entity_status = static_cast<traffic_simulator::EntityStatus>(*status); + entity_status.pose = updated_pose; + updated_pose.position = traffic_simulator::pose::updatePositionForLaneletTransition( + entity_status, next_lanelet_id, desired_twist.linear, desired_velocity_is_global, + time_step, hdmap_utils_ptr); } } return updated_pose; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 93b13227fd2..07097b76c2d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -60,12 +60,11 @@ 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; +auto updatePositionForLaneletTransition( + const traffic_simulator_msgs::msg::EntityStatus & entity_status, + const lanelet::Id next_lanelet_id, 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) -> geometry_msgs::msg::Point; // Relative msg::Pose auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index ef9cbd8ea18..0d4e522704f 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -575,24 +575,20 @@ 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; + // if it is the transition between lanelets: overwrite position to improve precision + if (estimated_next_canonicalized_lanelet_pose) { + const auto next_lanelet_id = + static_cast<LaneletPose>(estimated_next_canonicalized_lanelet_pose.value()).lanelet_id; + updated_status.pose.position = pose::updatePositionForLaneletTransition( + updated_status, next_lanelet_id, desired_velocity, desired_velocity_is_global, step_time, + hdmap_utils); } } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index dc5c353ac30..91bbd55f33c 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -142,61 +142,48 @@ 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 +/// @note this function does not modify the orientation of entity +auto updatePositionForLaneletTransition( + const traffic_simulator_msgs::msg::EntityStatus & entity_status, + const lanelet::Id next_lanelet_id, 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) -> geometry_msgs::msg::Point { 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 lanelet_pose = entity_status.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); + auto new_position = entity_status.pose.position; + + /// Transition to the next lanelet + if (math::geometry::norm(desired_velocity * step_time) > remaining_lanelet_length) { + // 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 Eigen::Vector3d global_velocity( + desired_velocity.x, desired_velocity.y, desired_velocity.z); + const Eigen::Quaterniond quaternion( + entity_status.pose.orientation.w, entity_status.pose.orientation.x, + entity_status.pose.orientation.y, entity_status.pose.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; + } + + LaneletPose result_lanelet_pose; + result_lanelet_pose.lanelet_id = next_lanelet_id; + result_lanelet_pose.s = displacement.x() - remaining_lanelet_length; + result_lanelet_pose.offset = lanelet_pose.offset + displacement.y(); + result_lanelet_pose.rpy = lanelet_pose.rpy; + new_position = toMapPose(result_lanelet_pose, hdmap_utils_ptr).position; } - result_lanelet_pose.offset = lanelet_pose.offset + lateral_d; - result_lanelet_pose.rpy = lanelet_pose.rpy; - return result_lanelet_pose; + return new_position; } auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) From 2ba428fe1499da2d49320c3863d3a53d47ebf757 Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Thu, 23 Jan 2025 11:48:54 +0100 Subject: [PATCH 29/38] ref(traffic_simulator) Refactor updateEntityPositionForLaneletTransition --- simulation/behavior_tree_plugin/src/action_node.cpp | 10 +++++----- .../include/traffic_simulator/utils/pose.hpp | 2 +- .../src/behavior/follow_trajectory.cpp | 8 ++++---- simulation/traffic_simulator/src/utils/pose.cpp | 10 +++++----- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 17e487539ff..6622e12889f 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -532,15 +532,15 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( 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) + // 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 + // Apply position change. /// @todo first determine global desired_velocity, calculate position change using it - // then pass the same global desired_velocity to adjustPoseForLaneletTransition() + // then pass the same global desired_velocity to adjustPoseForLaneletTransition(). const Eigen::Matrix3d rotation_matrix = math::geometry::getRotationMatrix(updated_pose.orientation); const auto translation = Eigen::Vector3d( @@ -555,14 +555,14 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance, hdmap_utils_ptr); - // if it is the transition between lanelets: overwrite position to improve precision + // If it is the transition between lanelets: overwrite position to improve precision. if (estimated_next_canonicalized_lanelet_pose) { const auto next_lanelet_id = static_cast<traffic_simulator::LaneletPose>( estimated_next_canonicalized_lanelet_pose.value()) .lanelet_id; auto entity_status = static_cast<traffic_simulator::EntityStatus>(*status); entity_status.pose = updated_pose; - updated_pose.position = traffic_simulator::pose::updatePositionForLaneletTransition( + updated_pose.position = traffic_simulator::pose::updateEntityPositionForLaneletTransition( entity_status, next_lanelet_id, desired_twist.linear, desired_velocity_is_global, time_step, hdmap_utils_ptr); } diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 07097b76c2d..380faf5fb92 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -60,7 +60,7 @@ auto transformRelativePoseToGlobal( const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose) -> geometry_msgs::msg::Pose; -auto updatePositionForLaneletTransition( +auto updateEntityPositionForLaneletTransition( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const lanelet::Id next_lanelet_id, const geometry_msgs::msg::Vector3 & desired_velocity, const bool desired_velocity_is_global, const double step_time, diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 0d4e522704f..78a2c6f1b3b 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -562,10 +562,10 @@ 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 + // 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 + // If there is a designed_velocity vector, set the orientation in the direction of it. const geometry_msgs::msg::Vector3 direction = geometry_msgs::build<geometry_msgs::msg::Vector3>() .x(0.0) @@ -582,11 +582,11 @@ auto makeUpdatedStatus( updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance, hdmap_utils); - // if it is the transition between lanelets: overwrite position to improve precision + // If it is the transition between lanelets: overwrite position to improve precision. if (estimated_next_canonicalized_lanelet_pose) { const auto next_lanelet_id = static_cast<LaneletPose>(estimated_next_canonicalized_lanelet_pose.value()).lanelet_id; - updated_status.pose.position = pose::updatePositionForLaneletTransition( + updated_status.pose.position = pose::updateEntityPositionForLaneletTransition( updated_status, next_lanelet_id, desired_velocity, desired_velocity_is_global, step_time, hdmap_utils); } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 91bbd55f33c..1eccfe3a710 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -142,8 +142,8 @@ auto transformRelativePoseToGlobal( return ret; } -/// @note this function does not modify the orientation of entity -auto updatePositionForLaneletTransition( +/// @note This function does not modify the orientation of entity. +auto updateEntityPositionForLaneletTransition( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const lanelet::Id next_lanelet_id, const geometry_msgs::msg::Vector3 & desired_velocity, const bool desired_velocity_is_global, const double step_time, @@ -157,12 +157,12 @@ auto updatePositionForLaneletTransition( hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s; auto new_position = entity_status.pose.position; - /// Transition to the next lanelet + // Transition to the next lanelet if the entity's displacement exceeds the remaining length if (math::geometry::norm(desired_velocity * step_time) > remaining_lanelet_length) { - // determine the displacement in the 2D lanelet coordinate system + // Determine the displacement in the 2D lanelet coordinate system Eigen::Vector2d displacement; if (desired_velocity_is_global) { - // transform desired (global) velocity to local velocity + // Transform desired (global) velocity to local velocity const Eigen::Vector3d global_velocity( desired_velocity.x, desired_velocity.y, desired_velocity.z); const Eigen::Quaterniond quaternion( From c6733e3461671380e06d6193b6fe9501afcfd561 Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Fri, 24 Jan 2025 16:11:49 +0100 Subject: [PATCH 30/38] fix(traffic_simulator) Improve lanelet transition handling for entity position updates --- .../behavior_tree_plugin/src/action_node.cpp | 23 +++---- .../include/traffic_simulator/utils/pose.hpp | 5 +- .../src/behavior/follow_trajectory.cpp | 16 ++--- .../traffic_simulator/src/utils/pose.cpp | 61 +++++++++++-------- 4 files changed, 58 insertions(+), 47 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 6622e12889f..83374ca08e6 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -513,7 +513,6 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( { using math::geometry::operator*; using math::geometry::operator+; - using math::geometry::operator-; using math::geometry::operator+=; constexpr bool desired_velocity_is_global{false}; @@ -532,15 +531,15 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( 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). + // 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. + // Apply position change /// @todo first determine global desired_velocity, calculate position change using it - // then pass the same global desired_velocity to adjustPoseForLaneletTransition(). + // then pass the same global desired_velocity to adjustPoseForLaneletTransition() const Eigen::Matrix3d rotation_matrix = math::geometry::getRotationMatrix(updated_pose.orientation); const auto translation = Eigen::Vector3d( @@ -549,22 +548,24 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const Eigen::Vector3d delta_position = rotation_matrix * translation; updated_pose.position = status->getMapPose().position + delta_position; + // 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, hdmap_utils_ptr); - - // If it is the transition between lanelets: overwrite position to improve precision. if (estimated_next_canonicalized_lanelet_pose) { + const auto entity_status = static_cast<traffic_simulator::EntityStatus>(*status); const auto next_lanelet_id = static_cast<traffic_simulator::LaneletPose>( estimated_next_canonicalized_lanelet_pose.value()) .lanelet_id; - auto entity_status = static_cast<traffic_simulator::EntityStatus>(*status); - entity_status.pose = updated_pose; - updated_pose.position = traffic_simulator::pose::updateEntityPositionForLaneletTransition( - entity_status, next_lanelet_id, desired_twist.linear, desired_velocity_is_global, - time_step, hdmap_utils_ptr); + if ( // Handle lanelet transition + const auto updated_position = + traffic_simulator::pose::updatePositionForLaneletTransition( + entity_status, next_lanelet_id, desired_twist.linear, desired_velocity_is_global, + time_step, hdmap_utils_ptr)) { + updated_pose.position = updated_position.value(); + } } } return updated_pose; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 380faf5fb92..369d81eaac2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -60,11 +60,12 @@ auto transformRelativePoseToGlobal( const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose) -> geometry_msgs::msg::Pose; -auto updateEntityPositionForLaneletTransition( +auto updatePositionForLaneletTransition( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const lanelet::Id next_lanelet_id, 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) -> geometry_msgs::msg::Point; + const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) + -> std::optional<geometry_msgs::msg::Point>; // Relative msg::Pose auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 78a2c6f1b3b..5f6341afa6e 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -562,10 +562,10 @@ 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. + // 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. + // If there is a designed_velocity vector, set the orientation in the direction of it const geometry_msgs::msg::Vector3 direction = geometry_msgs::build<geometry_msgs::msg::Vector3>() .x(0.0) @@ -575,20 +575,22 @@ auto makeUpdatedStatus( } }(); + // 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 estimated_next_canonicalized_lanelet_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( updated_status.pose, entity_status.bounding_box, include_crosswalk, matching_distance, hdmap_utils); - - // If it is the transition between lanelets: overwrite position to improve precision. if (estimated_next_canonicalized_lanelet_pose) { const auto next_lanelet_id = static_cast<LaneletPose>(estimated_next_canonicalized_lanelet_pose.value()).lanelet_id; - updated_status.pose.position = pose::updateEntityPositionForLaneletTransition( - updated_status, next_lanelet_id, desired_velocity, desired_velocity_is_global, step_time, - hdmap_utils); + if ( // Handle lanelet transition + const auto updated_position = pose::updatePositionForLaneletTransition( + entity_status, next_lanelet_id, desired_velocity, desired_velocity_is_global, step_time, + hdmap_utils)) { + updated_status.pose.position = updated_position.value(); + } } } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 1eccfe3a710..e8ea7673586 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -143,47 +143,54 @@ auto transformRelativePoseToGlobal( } /// @note This function does not modify the orientation of entity. -auto updateEntityPositionForLaneletTransition( +auto updatePositionForLaneletTransition( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const lanelet::Id next_lanelet_id, 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) -> geometry_msgs::msg::Point + const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) + -> std::optional<geometry_msgs::msg::Point> { using math::geometry::operator*; using math::geometry::operator+=; const auto lanelet_pose = entity_status.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 Eigen::Vector3d global_velocity( + desired_velocity.x, desired_velocity.y, desired_velocity.z); + const Eigen::Quaterniond quaternion( + entity_status.pose.orientation.w, entity_status.pose.orientation.x, + entity_status.pose.orientation.y, entity_status.pose.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(); + const auto remaining_lanelet_length = hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s; - auto new_position = entity_status.pose.position; - - // Transition to the next lanelet if the entity's displacement exceeds the remaining length - if (math::geometry::norm(desired_velocity * step_time) > remaining_lanelet_length) { - // 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 Eigen::Vector3d global_velocity( - desired_velocity.x, desired_velocity.y, desired_velocity.z); - const Eigen::Quaterniond quaternion( - entity_status.pose.orientation.w, entity_status.pose.orientation.x, - entity_status.pose.orientation.y, entity_status.pose.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 next_lanelet_longitudinal_d = longitudinal_d - remaining_lanelet_length; + if (longitudinal_d <= remaining_lanelet_length) { + return std::nullopt; + } 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_id)) { LaneletPose result_lanelet_pose; result_lanelet_pose.lanelet_id = next_lanelet_id; - result_lanelet_pose.s = displacement.x() - remaining_lanelet_length; - result_lanelet_pose.offset = lanelet_pose.offset + displacement.y(); + result_lanelet_pose.s = next_lanelet_longitudinal_d; + result_lanelet_pose.offset = lanelet_pose.offset + lateral_d; result_lanelet_pose.rpy = lanelet_pose.rpy; - new_position = toMapPose(result_lanelet_pose, hdmap_utils_ptr).position; + return toMapPose(result_lanelet_pose, hdmap_utils_ptr).position; + } else { + THROW_SIMULATION_ERROR( + "Next lanelet is too short: lanelet_id==", next_lanelet_id, " is shorter than ", + next_lanelet_longitudinal_d); } - return new_position; } auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) From 404f4288e2a1b940b21b9bfa8d2923530ef1b40b Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Mon, 27 Jan 2025 11:33:04 +0100 Subject: [PATCH 31/38] ref(traffic_simulator> Refactor code according to the lastest master branch changes --- .../behavior_tree_plugin/src/action_node.cpp | 18 ++++++-------- .../follow_polyline_trajectory_action.cpp | 1 + .../include/traffic_simulator/utils/pose.hpp | 8 +++---- .../src/behavior/follow_trajectory.cpp | 11 +++++---- .../traffic_simulator/src/utils/pose.cpp | 24 +++++++++---------- 5 files changed, 29 insertions(+), 33 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index c7980bc2063..b8c01c1d774 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -479,19 +479,18 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( 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) { + const geometry_msgs::msg::Twist & desired_twist, const double time_step) { 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; + delta_rotation = desired_twist.angular * 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 adjustPoseForLaneletTransition() + /// then pass the same global desired_velocity to adjustPoseForLaneletTransition() const Eigen::Matrix3d rotation_matrix = math::geometry::getRotationMatrix(updated_pose.orientation); const auto translation = Eigen::Vector3d( @@ -504,18 +503,16 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( 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); + updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance); if (estimated_next_canonicalized_lanelet_pose) { - const auto entity_status = static_cast<traffic_simulator::EntityStatus>(*status); const auto next_lanelet_id = static_cast<traffic_simulator::LaneletPose>( estimated_next_canonicalized_lanelet_pose.value()) .lanelet_id; if ( // Handle lanelet transition const auto updated_position = traffic_simulator::pose::updatePositionForLaneletTransition( - entity_status, next_lanelet_id, desired_twist.linear, desired_velocity_is_global, - time_step, hdmap_utils_ptr)) { + canonicalized_lanelet_pose.value(), next_lanelet_id, desired_twist.linear, + desired_velocity_is_global, time_step)) { updated_pose.position = updated_position.value(); } } @@ -532,8 +529,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( 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); + const auto pose_new = buildUpdatedPose(canonicalized_entity_status, twist_new, step_time); auto entity_status_updated = static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status); 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 d0f65ea7c80..8e722f5afb0 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,6 +63,7 @@ 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 diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 764dce4f483..352ed94c9eb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -61,11 +61,9 @@ auto transformRelativePoseToGlobal( -> geometry_msgs::msg::Pose; auto updatePositionForLaneletTransition( - const traffic_simulator_msgs::msg::EntityStatus & entity_status, - const lanelet::Id next_lanelet_id, 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) - -> std::optional<geometry_msgs::msg::Point>; + 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) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 0f2c0af28c9..39c687aff43 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -583,17 +583,18 @@ auto makeUpdatedStatus( // 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.pose, entity_status.bounding_box, include_crosswalk, matching_distance); 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 (estimated_next_canonicalized_lanelet_pose) { + 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 ( // Handle lanelet transition const auto updated_position = pose::updatePositionForLaneletTransition( - entity_status, next_lanelet_id, desired_velocity, desired_velocity_is_global, step_time, - hdmap_utils)) { + canonicalized_lanelet_pose.value(), next_lanelet_id, desired_velocity, + desired_velocity_is_global, step_time)) { updated_status.pose.position = updated_position.value(); } } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 4be4a539d97..c0424c2b015 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -179,16 +179,16 @@ auto transformRelativePoseToGlobal( /// @note This function does not modify the orientation of entity. auto updatePositionForLaneletTransition( - const traffic_simulator_msgs::msg::EntityStatus & entity_status, - const lanelet::Id next_lanelet_id, 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) - -> std::optional<geometry_msgs::msg::Point> + 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> { using math::geometry::operator*; using math::geometry::operator+=; - const auto lanelet_pose = entity_status.lanelet_pose; + const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose); + const auto map_pose = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose); + // Determine the displacement in the 2D lanelet coordinate system Eigen::Vector2d displacement; if (desired_velocity_is_global) { @@ -196,8 +196,8 @@ auto updatePositionForLaneletTransition( const Eigen::Vector3d global_velocity( desired_velocity.x, desired_velocity.y, desired_velocity.z); const Eigen::Quaterniond quaternion( - entity_status.pose.orientation.w, entity_status.pose.orientation.x, - entity_status.pose.orientation.y, entity_status.pose.orientation.z); + map_pose.orientation.w, map_pose.orientation.x, map_pose.orientation.y, + map_pose.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; @@ -209,25 +209,25 @@ auto updatePositionForLaneletTransition( const auto lateral_d = displacement.y(); const auto remaining_lanelet_length = - hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s; + lanelet_wrapper::lanelet_map::laneletLength(lanelet_pose.lanelet_id) - lanelet_pose.s; const auto next_lanelet_longitudinal_d = longitudinal_d - remaining_lanelet_length; if (longitudinal_d <= remaining_lanelet_length) { return std::nullopt; } 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_id)) { + next_lanelet_longitudinal_d < lanelet_wrapper::lanelet_map::laneletLength(next_lanelet_id)) { LaneletPose result_lanelet_pose; result_lanelet_pose.lanelet_id = next_lanelet_id; result_lanelet_pose.s = next_lanelet_longitudinal_d; result_lanelet_pose.offset = lanelet_pose.offset + lateral_d; result_lanelet_pose.rpy = lanelet_pose.rpy; - return toMapPose(result_lanelet_pose, hdmap_utils_ptr).position; + return toMapPose(result_lanelet_pose).position; } else { THROW_SIMULATION_ERROR( "Next lanelet is too short: lanelet_id==", next_lanelet_id, " is shorter than ", next_lanelet_longitudinal_d); } } - + /// @note Relative msg::Pose auto isAltitudeMatching( const CanonicalizedLaneletPose & lanelet_pose, From 44870e38d0f45345593d86fa622ac459b29b7be4 Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Mon, 27 Jan 2025 11:36:02 +0100 Subject: [PATCH 32/38] reref(behavior_tree_plugin) Update commment --- simulation/behavior_tree_plugin/src/action_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index b8c01c1d774..3d7d021e296 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -490,7 +490,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( // Apply position change /// @todo first determine global desired_velocity, calculate position change using it - /// then pass the same global desired_velocity to adjustPoseForLaneletTransition() + /// 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( From d81796b3671a76a296fc841e998085ce48b17899 Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Mon, 27 Jan 2025 11:58:02 +0100 Subject: [PATCH 33/38] ref(traffic_simulator): Refactor comments --- simulation/behavior_tree_plugin/src/action_node.cpp | 8 +++----- simulation/traffic_simulator/src/utils/pose.cpp | 8 +++----- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 3d7d021e296..6414b941c09 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -254,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 ~1.5708 -> 1.5708, entity is a candidate of front entity. if ( std::fabs(math::geometry::convertQuaternionToEulerAngle(quat).z) <= boost::math::constants::half_pi<double>()) { @@ -489,8 +487,8 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( 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 updatePositionForLaneletTransition() + // @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( diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index c0424c2b015..c4bae8cb08d 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -187,12 +187,12 @@ auto updatePositionForLaneletTransition( using math::geometry::operator+=; const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose); - const auto map_pose = static_cast<geometry_msgs::msg::Pose>(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 map_pose = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose); const Eigen::Vector3d global_velocity( desired_velocity.x, desired_velocity.y, desired_velocity.z); const Eigen::Quaterniond quaternion( @@ -402,10 +402,8 @@ auto transformToCanonicalizedLaneletPose( map_pose, bounding_box, unique_route_lanelets, include_crosswalk, matching_distance)) { return canonicalized_lanelet_pose; } - /** - * @note Hard coded parameter. 2.0 is a matching threshold for lanelet. - * In this branch, the algorithm only consider entity pose. - */ + /// @note Hard coded parameter. 2.0 is a matching threshold for lanelet. + /// In this branch, the algorithm only consider entity pose. if ( const auto lanelet_pose = lanelet_wrapper::pose::toLaneletPose(map_pose, include_crosswalk, 2.0)) { From 79e78187e94d521ad61c6cbc97477c48864a9e62 Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Mon, 27 Jan 2025 12:27:48 +0100 Subject: [PATCH 34/38] ref(traffic_simulator): Refactor comments --- .../behavior_tree_plugin/src/action_node.cpp | 14 +- .../src/behavior/follow_trajectory.cpp | 244 ++++++++---------- .../traffic_simulator/src/utils/pose.cpp | 22 +- 3 files changed, 123 insertions(+), 157 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 6414b941c09..f2d7211558c 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -254,7 +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>()) { @@ -480,15 +480,15 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const geometry_msgs::msg::Twist & desired_twist, const double time_step) { geometry_msgs::msg::Pose updated_pose; - // Apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s) + /// @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; - // Apply position change - // @todo first determine global desired_velocity, calculate position change using it - // then pass the same global desired_velocity to updatePositionForLaneletTransition() + /// @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( @@ -497,7 +497,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const Eigen::Vector3d delta_position = rotation_matrix * translation; updated_pose.position = status->getMapPose().position + delta_position; - // If it is the transition between lanelets: overwrite position to improve precision + /// @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( @@ -506,7 +506,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const auto next_lanelet_id = static_cast<traffic_simulator::LaneletPose>( estimated_next_canonicalized_lanelet_pose.value()) .lanelet_id; - if ( // Handle lanelet transition + if ( /// @note Handle lanelet transition const auto updated_position = traffic_simulator::pose::updatePositionForLaneletTransition( canonicalized_lanelet_pose.value(), next_lanelet_id, desired_twist.linear, diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 39c687aff43..eac06de886f 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -101,23 +101,21 @@ auto makeUpdatedStatus( }; auto discard_the_front_waypoint_and_recurse = [&]() { - /* - The OpenSCENARIO standard does not define the behavior when the value of - Timing.domainAbsoluteRelative is "relative". The standard only states - "Definition of time value context as either absolute or relative", and - it is completely unclear when the relative time starts. - - This implementation has interpreted the specification as follows: - Relative time starts from the start of FollowTrajectoryAction or from - the time of reaching the previous "waypoint with arrival time". - - Note: not std::isnan(polyline_trajectory.base_time) means - "Timing.domainAbsoluteRelative is relative". - - Note: not std::isnan(polyline_trajectory.shape.vertices.front().time) - means "The waypoint about to be popped is the waypoint with the - specified arrival time". - */ + /// @note The OpenSCENARIO standard does not define the behavior when the value of + /// Timing.domainAbsoluteRelative is "relative". The standard only states + /// "Definition of time value context as either absolute or relative", and + /// it is completely unclear when the relative time starts. + /// + /// This implementation has interpreted the specification as follows: + /// Relative time starts from the start of FollowTrajectoryAction or from + /// the time of reaching the previous "waypoint with arrival time". + /// + /// Note: not std::isnan(polyline_trajectory.base_time) means + /// "Timing.domainAbsoluteRelative is relative". + /// + /// Note: not std::isnan(polyline_trajectory.shape.vertices.front().time) + /// means "The waypoint about to be popped is the waypoint with the + /// specified arrival time". if ( not std::isnan(polyline_trajectory.base_time) and not std::isnan(polyline_trajectory.shape.vertices.front().time)) { @@ -150,13 +148,11 @@ auto makeUpdatedStatus( std::prev(polyline_trajectory.shape.vertices.end()); }; - /* - The following code implements the steering behavior known as "seek". See - "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more - information. - - See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters - */ + /// @note The following code implements the steering behavior known as "seek". See + /// "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more + /// information. + /// + /// See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters if (polyline_trajectory.shape.vertices.empty()) { return std::nullopt; } else if (const auto position = entity_status.pose.position; any(is_infinity_or_nan, position)) { @@ -166,11 +162,9 @@ auto makeUpdatedStatus( std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [", position.x, ", ", position.y, ", ", position.z, "]."); } else if ( - /* - We've made sure that polyline_trajectory.shape.vertices is not empty, so - a reference to vertices.front() always succeeds. vertices.front() is - referenced only this once in this member function. - */ + /// @note We've made sure that polyline_trajectory.shape.vertices is not empty, so + /// a reference to vertices.front() always succeeds. vertices.front() is + /// referenced only this once in this member function. const auto target_position = polyline_trajectory.shape.vertices.front().position.position; any(is_infinity_or_nan, target_position)) { throw common::Error( @@ -180,30 +174,24 @@ auto makeUpdatedStatus( "'s target position coordinate value contains NaN or infinity. The value is [", target_position.x, ", ", target_position.y, ", ", target_position.z, "]."); } else if ( - /* - If not dynamic_constraints_ignorable, the linear distance should cause - problems. - */ + /// @note If not dynamic_constraints_ignorable, the linear distance should cause + /// problems. const auto [distance_to_front_waypoint, remaining_time_to_front_waypoint] = std::make_tuple( distance_along_lanelet(position, target_position), (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + polyline_trajectory.shape.vertices.front().time - entity_status.time); - /* - This clause is to avoid division-by-zero errors in later clauses with - distance_to_front_waypoint as the denominator if the distance - miraculously becomes zero. - */ + /// @note This clause is to avoid division-by-zero errors in later clauses with + /// distance_to_front_waypoint as the denominator if the distance + /// miraculously becomes zero. isDefinitelyLessThan(distance_to_front_waypoint, std::numeric_limits<double>::epsilon())) { return discard_the_front_waypoint_and_recurse(); } else if ( const auto [distance, remaining_time] = [&]() { - /* - Note for anyone working on adding support for followingMode follow - to this function (FollowPolylineTrajectoryAction::tick) in the - future: if followingMode is follow, this distance calculation may be - inappropriate. - */ + /// @note Note for anyone working on adding support for followingMode follow + /// to this function (FollowPolylineTrajectoryAction::tick) in the + /// future: if followingMode is follow, this distance calculation may be + /// inappropriate. auto total_distance_to = [&](auto last) { auto total_distance = 0.0; for (auto iter = std::begin(polyline_trajectory.shape.vertices); @@ -221,28 +209,26 @@ auto makeUpdatedStatus( (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + first_waypoint_with_arrival_time_specified()->time - entity_status.time; - /* - The condition below should ideally be remaining_time < 0. - - The simulator runs at a constant frame rate, so the step time is - 1/FPS. If the simulation time is an accumulation of step times - expressed as rational numbers, times that are integer multiples - of the frame rate will always be exact integer seconds. - Therefore, the timing of remaining_time == 0 always exists, and - the velocity planning of this member function (tick) aims to - reach the waypoint exactly at that timing. So the ideal timeout - condition is remaining_time < 0. - - But actually the step time is expressed as a float and the - simulation time is its accumulation. As a result, it is not - guaranteed that there will be times when the simulation time is - exactly zero. For example, remaining_time == -0.00006 and it was - judged to be out of time. - - For the above reasons, the condition is remaining_time < - -step_time. In other words, the conditions are such that a delay - of 1 step time is allowed. - */ + /// @note The condition below should ideally be remaining_time < 0. + /// + /// The simulator runs at a constant frame rate, so the step time is + /// 1/FPS. If the simulation time is an accumulation of step times + /// expressed as rational numbers, times that are integer multiples + /// of the frame rate will always be exact integer seconds. + /// Therefore, the timing of remaining_time == 0 always exists, and + /// the velocity planning of this member function (tick) aims to + /// reach the waypoint exactly at that timing. So the ideal timeout + /// condition is remaining_time < 0. + /// + /// But actually the step time is expressed as a float and the + /// simulation time is its accumulation. As a result, it is not + /// guaranteed that there will be times when the simulation time is + /// exactly zero. For example, remaining_time == -0.00006 and it was + /// judged to be out of time. + /// + /// For the above reasons, the condition is remaining_time < + /// -step_time. In other words, the conditions are such that a delay + /// of 1 step time is allowed. remaining_time < -step_time) { throw common::Error( "Vehicle ", std::quoted(entity_status.name), @@ -304,40 +290,36 @@ auto makeUpdatedStatus( std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", speed, ". "); } else if ( - /* - The controller provides the ability to calculate acceleration using constraints from the - behavior_parameter. The value is_breaking_waypoint() determines whether the calculated - acceleration takes braking into account - it is true if the nearest waypoint with the - specified time is the last waypoint or the nearest waypoint without the specified time is the - last waypoint. - - If an arrival time was specified for any of the remaining waypoints, priority is given to - meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can - be met. - - However, the controller allows passing target_speed as a speed which is followed by the - controller. target_speed is passed only if no arrival time was specified for any of the - remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is - not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the - behaviour_parameter. - */ + /// @note The controller provides the ability to calculate acceleration using constraints from the + /// behavior_parameter. The value is_breaking_waypoint() determines whether the calculated + /// acceleration takes braking into account - it is true if the nearest waypoint with the + /// specified time is the last waypoint or the nearest waypoint without the specified time is the + /// last waypoint. + /// + /// If an arrival time was specified for any of the remaining waypoints, priority is given to + /// meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can + /// be met. + /// + /// However, the controller allows passing target_speed as a speed which is followed by the + /// controller. target_speed is passed only if no arrival time was specified for any of the + /// remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is + /// not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the + /// behaviour_parameter. const auto follow_waypoint_controller = FollowWaypointController( behavior_parameter, step_time, is_breaking_waypoint(), std::isinf(remaining_time) ? target_speed : std::nullopt); false) { } else if ( - /* - The desired acceleration is the acceleration at which the destination - can be reached exactly at the specified time (= time remaining at zero). - - The desired acceleration is calculated to the nearest waypoint with a specified arrival time. - It is calculated in such a way as to reach a constant linear speed as quickly as possible, - ensuring arrival at a waypoint at the precise time and with the shortest possible distance. - More precisely, the controller selects acceleration to minimize the distance to the waypoint - that will be reached in a time step defined as the expected arrival time. - In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, - with linear speed equal to zero and acceleration equal to zero. - */ + /// @note The desired acceleration is the acceleration at which the destination + /// can be reached exactly at the specified time (= time remaining at zero). + /// + /// The desired acceleration is calculated to the nearest waypoint with a specified arrival time. + /// It is calculated in such a way as to reach a constant linear speed as quickly as possible, + /// ensuring arrival at a waypoint at the precise time and with the shortest possible distance. + /// More precisely, the controller selects acceleration to minimize the distance to the waypoint + /// that will be reached in a time step defined as the expected arrival time. + /// In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, + /// with linear speed equal to zero and acceleration equal to zero. const auto desired_acceleration = [&]() -> double { try { return follow_waypoint_controller.getAcceleration( @@ -365,15 +347,13 @@ auto makeUpdatedStatus( desired_speed, ". "); } else if (const auto desired_velocity = [&]() { - /* - Note: The followingMode in OpenSCENARIO is passed as - variable dynamic_constraints_ignorable. the value of the - variable is `followingMode == position`. - */ + /// @note The followingMode in OpenSCENARIO is passed as + /// variable dynamic_constraints_ignorable. the value of the + /// variable is `followingMode == position`. 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( @@ -386,14 +366,12 @@ auto makeUpdatedStatus( .y(std::cos(pitch) * std::sin(yaw) * desired_speed) .z(std::sin(pitch) * desired_speed); } else { - /* - Note: The vector returned if - dynamic_constraints_ignorable == true ignores parameters - such as the maximum rudder angle of the vehicle entry. In - this clause, such parameters must be respected and the - rotation angle difference of the z-axis center of the - vector must be kept below a certain value. - */ + /// @note The vector returned if + /// dynamic_constraints_ignorable == true ignores parameters + /// such as the maximum rudder angle of the vehicle entry. In + /// this clause, such parameters must be respected and the + /// rotation angle difference of the z-axis center of the + /// vector must be kept below a certain value. throw common::SimulationError( "The followingMode is only supported for position."); } @@ -515,37 +493,29 @@ 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(); } } - /* - If there is insufficient time left for the next calculation step. - The value of step_time/2 is compared, as the remaining time is affected by floating point - inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - - 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value - here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next - step is possible (remaining_time_to_front_waypoint is almost zero). - */ + /// @note If there is insufficient time left for the next calculation step. + /// The value of step_time/2 is compared, as the remaining time is affected by floating point + /// inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - + /// 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value + /// here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next + /// step is possible (remaining_time_to_front_waypoint is almost zero). } else if (isDefinitelyLessThan(remaining_time_to_front_waypoint, step_time / 2.0)) { if (follow_waypoint_controller.areConditionsOfArrivalMet( acceleration, speed, distance_to_front_waypoint)) { @@ -561,26 +531,24 @@ auto makeUpdatedStatus( } } - /* - Note: If obstacle avoidance is to be implemented, the steering behavior - known by the name "collision avoidance" should be synthesized here into - steering. - */ + /// @note If obstacle avoidance is to be implemented, the steering behavior + /// known by the name "collision avoidance" should be synthesized here into + /// steering. auto updated_status = entity_status; updated_status.pose.position += desired_velocity * step_time; 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); } }(); - // If it is the transition between lanelets: overwrite position to improve precision + /// @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( @@ -591,7 +559,7 @@ auto makeUpdatedStatus( 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 ( // Handle lanelet transition + 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)) { diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index c4bae8cb08d..0904ee42f63 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -188,10 +188,10 @@ auto updatePositionForLaneletTransition( const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose); - // Determine the displacement in the 2D lanelet coordinate system + /// @note Determine the displacement in the 2D lanelet coordinate system Eigen::Vector2d displacement; if (desired_velocity_is_global) { - // Transform desired (global) velocity to local velocity + /// @note Transform desired (global) velocity to local velocity const auto map_pose = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose); const Eigen::Vector3d global_velocity( desired_velocity.x, desired_velocity.y, desired_velocity.z); @@ -213,7 +213,7 @@ auto updatePositionForLaneletTransition( const auto next_lanelet_longitudinal_d = longitudinal_d - remaining_lanelet_length; if (longitudinal_d <= remaining_lanelet_length) { return std::nullopt; - } else if ( // if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible + } else if ( /// @note if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible next_lanelet_longitudinal_d < lanelet_wrapper::lanelet_map::laneletLength(next_lanelet_id)) { LaneletPose result_lanelet_pose; result_lanelet_pose.lanelet_id = next_lanelet_id; @@ -290,8 +290,8 @@ auto relativeLaneletPose( constexpr bool include_opposite_direction{true}; LaneletPose position = quietNaNLaneletPose(); - // here the s and offset are intentionally assigned independently, even if - // it is not possible to calculate one of them - it happens that one is sufficient + /// @note here the s and offset are intentionally assigned independently, even if + /// it is not possible to calculate one of them - it happens that one is sufficient if ( const auto longitudinal_distance = longitudinalDistance( from, to, include_adjacent_lanelet, include_opposite_direction, routing_configuration, @@ -319,8 +319,8 @@ auto boundingBoxRelativeLaneletPose( constexpr bool include_opposite_direction{true}; LaneletPose position = quietNaNLaneletPose(); - // here the s and offset are intentionally assigned independently, even if - // it is not possible to calculate one of them - it happens that one is sufficient + /// @note here the s and offset are intentionally assigned independently, even if + /// it is not possible to calculate one of them - it happens that one is sufficient if ( const auto longitudinal_bounding_box_distance = boundingBoxLaneLongitudinalDistance( from, from_bounding_box, to, to_bounding_box, include_adjacent_lanelet, @@ -386,11 +386,9 @@ auto isAtEndOfLanelets( namespace pedestrian { -/* - This function has been moved from pedestrian_action_node and modified, - in case of inconsistency please compare in original: - https://github.com/tier4/scenario_simulator_v2/blob/090a8d08bcb065d293a530cf641a953edf311f9f/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L67-L128 -*/ +/// @note This function has been moved from pedestrian_action_node and modified, +/// in case of inconsistency please compare in original: +/// https://github.com/tier4/scenario_simulator_v2/blob/090a8d08bcb065d293a530cf641a953edf311f9f/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L67-L128 auto transformToCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, From d38d3505126cf6e988f097124e1bf9a30101822b Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Mon, 27 Jan 2025 13:57:42 +0100 Subject: [PATCH 35/38] ref(traffic_simulator): Refactor comments --- .../src/behavior/follow_trajectory.cpp | 216 ++++++++++-------- .../traffic_simulator/src/utils/pose.cpp | 8 +- 2 files changed, 126 insertions(+), 98 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index eac06de886f..26f75ce79bf 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -101,21 +101,23 @@ auto makeUpdatedStatus( }; auto discard_the_front_waypoint_and_recurse = [&]() { - /// @note The OpenSCENARIO standard does not define the behavior when the value of - /// Timing.domainAbsoluteRelative is "relative". The standard only states - /// "Definition of time value context as either absolute or relative", and - /// it is completely unclear when the relative time starts. - /// - /// This implementation has interpreted the specification as follows: - /// Relative time starts from the start of FollowTrajectoryAction or from - /// the time of reaching the previous "waypoint with arrival time". - /// - /// Note: not std::isnan(polyline_trajectory.base_time) means - /// "Timing.domainAbsoluteRelative is relative". - /// - /// Note: not std::isnan(polyline_trajectory.shape.vertices.front().time) - /// means "The waypoint about to be popped is the waypoint with the - /// specified arrival time". + /* + The OpenSCENARIO standard does not define the behavior when the value of + Timing.domainAbsoluteRelative is "relative". The standard only states + "Definition of time value context as either absolute or relative", and + it is completely unclear when the relative time starts. + + This implementation has interpreted the specification as follows: + Relative time starts from the start of FollowTrajectoryAction or from + the time of reaching the previous "waypoint with arrival time". + + Note: not std::isnan(polyline_trajectory.base_time) means + "Timing.domainAbsoluteRelative is relative". + + Note: not std::isnan(polyline_trajectory.shape.vertices.front().time) + means "The waypoint about to be popped is the waypoint with the + specified arrival time". + */ if ( not std::isnan(polyline_trajectory.base_time) and not std::isnan(polyline_trajectory.shape.vertices.front().time)) { @@ -148,11 +150,13 @@ auto makeUpdatedStatus( std::prev(polyline_trajectory.shape.vertices.end()); }; - /// @note The following code implements the steering behavior known as "seek". See - /// "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more - /// information. - /// - /// See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters + /* + The following code implements the steering behavior known as "seek". See + "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more + information. + + See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters + */ if (polyline_trajectory.shape.vertices.empty()) { return std::nullopt; } else if (const auto position = entity_status.pose.position; any(is_infinity_or_nan, position)) { @@ -162,9 +166,11 @@ auto makeUpdatedStatus( std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [", position.x, ", ", position.y, ", ", position.z, "]."); } else if ( - /// @note We've made sure that polyline_trajectory.shape.vertices is not empty, so - /// a reference to vertices.front() always succeeds. vertices.front() is - /// referenced only this once in this member function. + /* + We've made sure that polyline_trajectory.shape.vertices is not empty, so + a reference to vertices.front() always succeeds. vertices.front() is + referenced only this once in this member function. + */ const auto target_position = polyline_trajectory.shape.vertices.front().position.position; any(is_infinity_or_nan, target_position)) { throw common::Error( @@ -174,24 +180,30 @@ auto makeUpdatedStatus( "'s target position coordinate value contains NaN or infinity. The value is [", target_position.x, ", ", target_position.y, ", ", target_position.z, "]."); } else if ( - /// @note If not dynamic_constraints_ignorable, the linear distance should cause - /// problems. + /* + If not dynamic_constraints_ignorable, the linear distance should cause + problems. + */ const auto [distance_to_front_waypoint, remaining_time_to_front_waypoint] = std::make_tuple( distance_along_lanelet(position, target_position), (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + polyline_trajectory.shape.vertices.front().time - entity_status.time); - /// @note This clause is to avoid division-by-zero errors in later clauses with - /// distance_to_front_waypoint as the denominator if the distance - /// miraculously becomes zero. + /* + This clause is to avoid division-by-zero errors in later clauses with + distance_to_front_waypoint as the denominator if the distance + miraculously becomes zero. + */ isDefinitelyLessThan(distance_to_front_waypoint, std::numeric_limits<double>::epsilon())) { return discard_the_front_waypoint_and_recurse(); } else if ( const auto [distance, remaining_time] = [&]() { - /// @note Note for anyone working on adding support for followingMode follow - /// to this function (FollowPolylineTrajectoryAction::tick) in the - /// future: if followingMode is follow, this distance calculation may be - /// inappropriate. + /* + Note for anyone working on adding support for followingMode follow + to this function (FollowPolylineTrajectoryAction::tick) in the + future: if followingMode is follow, this distance calculation may be + inappropriate. + */ auto total_distance_to = [&](auto last) { auto total_distance = 0.0; for (auto iter = std::begin(polyline_trajectory.shape.vertices); @@ -209,26 +221,28 @@ auto makeUpdatedStatus( (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + first_waypoint_with_arrival_time_specified()->time - entity_status.time; - /// @note The condition below should ideally be remaining_time < 0. - /// - /// The simulator runs at a constant frame rate, so the step time is - /// 1/FPS. If the simulation time is an accumulation of step times - /// expressed as rational numbers, times that are integer multiples - /// of the frame rate will always be exact integer seconds. - /// Therefore, the timing of remaining_time == 0 always exists, and - /// the velocity planning of this member function (tick) aims to - /// reach the waypoint exactly at that timing. So the ideal timeout - /// condition is remaining_time < 0. - /// - /// But actually the step time is expressed as a float and the - /// simulation time is its accumulation. As a result, it is not - /// guaranteed that there will be times when the simulation time is - /// exactly zero. For example, remaining_time == -0.00006 and it was - /// judged to be out of time. - /// - /// For the above reasons, the condition is remaining_time < - /// -step_time. In other words, the conditions are such that a delay - /// of 1 step time is allowed. + /* + The condition below should ideally be remaining_time < 0. + + The simulator runs at a constant frame rate, so the step time is + 1/FPS. If the simulation time is an accumulation of step times + expressed as rational numbers, times that are integer multiples + of the frame rate will always be exact integer seconds. + Therefore, the timing of remaining_time == 0 always exists, and + the velocity planning of this member function (tick) aims to + reach the waypoint exactly at that timing. So the ideal timeout + condition is remaining_time < 0. + + But actually the step time is expressed as a float and the + simulation time is its accumulation. As a result, it is not + guaranteed that there will be times when the simulation time is + exactly zero. For example, remaining_time == -0.00006 and it was + judged to be out of time. + + For the above reasons, the condition is remaining_time < + -step_time. In other words, the conditions are such that a delay + of 1 step time is allowed. + */ remaining_time < -step_time) { throw common::Error( "Vehicle ", std::quoted(entity_status.name), @@ -290,36 +304,40 @@ auto makeUpdatedStatus( std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", speed, ". "); } else if ( - /// @note The controller provides the ability to calculate acceleration using constraints from the - /// behavior_parameter. The value is_breaking_waypoint() determines whether the calculated - /// acceleration takes braking into account - it is true if the nearest waypoint with the - /// specified time is the last waypoint or the nearest waypoint without the specified time is the - /// last waypoint. - /// - /// If an arrival time was specified for any of the remaining waypoints, priority is given to - /// meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can - /// be met. - /// - /// However, the controller allows passing target_speed as a speed which is followed by the - /// controller. target_speed is passed only if no arrival time was specified for any of the - /// remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is - /// not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the - /// behaviour_parameter. + /* + The controller provides the ability to calculate acceleration using constraints from the + behavior_parameter. The value is_breaking_waypoint() determines whether the calculated + acceleration takes braking into account - it is true if the nearest waypoint with the + specified time is the last waypoint or the nearest waypoint without the specified time is the + last waypoint. + + If an arrival time was specified for any of the remaining waypoints, priority is given to + meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can + be met. + + However, the controller allows passing target_speed as a speed which is followed by the + controller. target_speed is passed only if no arrival time was specified for any of the + remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is + not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the + behaviour_parameter. + */ const auto follow_waypoint_controller = FollowWaypointController( behavior_parameter, step_time, is_breaking_waypoint(), std::isinf(remaining_time) ? target_speed : std::nullopt); false) { } else if ( - /// @note The desired acceleration is the acceleration at which the destination - /// can be reached exactly at the specified time (= time remaining at zero). - /// - /// The desired acceleration is calculated to the nearest waypoint with a specified arrival time. - /// It is calculated in such a way as to reach a constant linear speed as quickly as possible, - /// ensuring arrival at a waypoint at the precise time and with the shortest possible distance. - /// More precisely, the controller selects acceleration to minimize the distance to the waypoint - /// that will be reached in a time step defined as the expected arrival time. - /// In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, - /// with linear speed equal to zero and acceleration equal to zero. + /* + The desired acceleration is the acceleration at which the destination + can be reached exactly at the specified time (= time remaining at zero). + + The desired acceleration is calculated to the nearest waypoint with a specified arrival time. + It is calculated in such a way as to reach a constant linear speed as quickly as possible, + ensuring arrival at a waypoint at the precise time and with the shortest possible distance. + More precisely, the controller selects acceleration to minimize the distance to the waypoint + that will be reached in a time step defined as the expected arrival time. + In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, + with linear speed equal to zero and acceleration equal to zero. + */ const auto desired_acceleration = [&]() -> double { try { return follow_waypoint_controller.getAcceleration( @@ -347,9 +365,11 @@ auto makeUpdatedStatus( desired_speed, ". "); } else if (const auto desired_velocity = [&]() { - /// @note The followingMode in OpenSCENARIO is passed as - /// variable dynamic_constraints_ignorable. the value of the - /// variable is `followingMode == position`. + /* + Note: The followingMode in OpenSCENARIO is passed as + variable dynamic_constraints_ignorable. the value of the + variable is `followingMode == position`. + */ if (polyline_trajectory.dynamic_constraints_ignorable) { const auto dx = target_position.x - position.x; const auto dy = target_position.y - position.y; @@ -366,12 +386,14 @@ auto makeUpdatedStatus( .y(std::cos(pitch) * std::sin(yaw) * desired_speed) .z(std::sin(pitch) * desired_speed); } else { - /// @note The vector returned if - /// dynamic_constraints_ignorable == true ignores parameters - /// such as the maximum rudder angle of the vehicle entry. In - /// this clause, such parameters must be respected and the - /// rotation angle difference of the z-axis center of the - /// vector must be kept below a certain value. + /* + Note: The vector returned if + dynamic_constraints_ignorable == true ignores parameters + such as the maximum rudder angle of the vehicle entry. In + this clause, such parameters must be respected and the + rotation angle difference of the z-axis center of the + vector must be kept below a certain value. + */ throw common::SimulationError( "The followingMode is only supported for position."); } @@ -510,12 +532,14 @@ auto makeUpdatedStatus( return discard_the_front_waypoint_and_recurse(); } } - /// @note If there is insufficient time left for the next calculation step. - /// The value of step_time/2 is compared, as the remaining time is affected by floating point - /// inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - - /// 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value - /// here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next - /// step is possible (remaining_time_to_front_waypoint is almost zero). + /* + If there is insufficient time left for the next calculation step. + The value of step_time/2 is compared, as the remaining time is affected by floating point + inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - + 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value + here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next + step is possible (remaining_time_to_front_waypoint is almost zero). + */ } else if (isDefinitelyLessThan(remaining_time_to_front_waypoint, step_time / 2.0)) { if (follow_waypoint_controller.areConditionsOfArrivalMet( acceleration, speed, distance_to_front_waypoint)) { @@ -531,9 +555,11 @@ auto makeUpdatedStatus( } } - /// @note If obstacle avoidance is to be implemented, the steering behavior - /// known by the name "collision avoidance" should be synthesized here into - /// steering. + /* + Note: If obstacle avoidance is to be implemented, the steering behavior + known by the name "collision avoidance" should be synthesized here into + steering. + */ auto updated_status = entity_status; updated_status.pose.position += desired_velocity * step_time; diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 0904ee42f63..55c0d1304d8 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -386,9 +386,11 @@ auto isAtEndOfLanelets( namespace pedestrian { -/// @note This function has been moved from pedestrian_action_node and modified, -/// in case of inconsistency please compare in original: -/// https://github.com/tier4/scenario_simulator_v2/blob/090a8d08bcb065d293a530cf641a953edf311f9f/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L67-L128 +/* + This function has been moved from pedestrian_action_node and modified, + in case of inconsistency please compare in original: + https://github.com/tier4/scenario_simulator_v2/blob/090a8d08bcb065d293a530cf641a953edf311f9f/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L67-L128 +*/ auto transformToCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, From 336a7eedabb7dc4a6a6e7f6d74dbbae987b3e212 Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Wed, 29 Jan 2025 00:14:28 +0100 Subject: [PATCH 36/38] fix(traffic_simulator) Fix an issue with follow trajectory action - Updated makeUpdatedStatus to fix an issue with toCanonicalizedLaneletPose --- .../traffic_simulator/src/behavior/follow_trajectory.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 26f75ce79bf..9cfe77ab873 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -577,8 +577,8 @@ auto makeUpdatedStatus( /// @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.pose, entity_status.bounding_box, include_crosswalk, matching_distance); + 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); From 6d7aa6a7d841703c1a825abb5ac67aeccaf56077 Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Wed, 29 Jan 2025 15:04:56 +0100 Subject: [PATCH 37/38] fix(traffic_simulator): Fix an issue with lanelet transition --- .../traffic_simulator/src/behavior/follow_trajectory.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 9cfe77ab873..196514fde23 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -580,8 +580,7 @@ auto makeUpdatedStatus( 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); + traffic_simulator::pose::toCanonicalizedLaneletPose(updated_status.pose, include_crosswalk); 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; From d2d20f6c9aa039ab6fe422fe73bd02558d53531b Mon Sep 17 00:00:00 2001 From: SzymonParapura <szymon.parapura@robotec.ai> Date: Tue, 4 Feb 2025 10:54:21 +0100 Subject: [PATCH 38/38] ref(behavior_tree_plugin): Refactored calculateUpdatedEntityStatusInWorldFrame --- simulation/behavior_tree_plugin/src/action_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index a364ae49458..5f26d0eca7b 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -518,7 +518,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( const auto matching_distance = default_matching_distance_for_lanelet_pose_calculation; - const auto buildUpdatedPose = + const auto build_updated_pose = [&include_crosswalk, &matching_distance]( const std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> & status, const geometry_msgs::msg::Twist & desired_twist, const double time_step) { @@ -571,7 +571,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( 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); + const auto pose_new = build_updated_pose(canonicalized_entity_status, twist_new, step_time); auto entity_status_updated = static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status);