From c5b8b8bf6a7e14972f9fe077016870f107aaed75 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Wed, 18 Dec 2024 13:05:35 +0100 Subject: [PATCH] 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 | 4 +- 4 files changed, 108 insertions(+), 1 deletion(-) 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 +#include + +namespace math +{ +namespace geometry +{ +template ::value, std::nullptr_t> = nullptr> +auto norm(const T & q) +{ + return std::hypot(std::hypot(q.w, q.x), std::hypot(q.y, q.z)); +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__NORM_HPP_ diff --git a/common/math/geometry/include/geometry/quaternion/normalize.hpp b/common/math/geometry/include/geometry/quaternion/normalize.hpp new file mode 100644 index 00000000000..9b819b7bf3f --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/normalize.hpp @@ -0,0 +1,43 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEOMETRY__QUATERNION__NORMALIZE_HPP_ +#define GEOMETRY__QUATERNION__NORMALIZE_HPP_ + +#include +#include +#include +#include + +namespace math +{ +namespace geometry +{ +template ::value, std::nullptr_t> = nullptr> +auto normalize(const T & q) +{ + const auto n = norm(q); + if (std::fabs(n) <= std::numeric_limits::epsilon()) { + THROW_SIMULATION_ERROR( + "Norm of quaternion (", q.w, ",", q.x, ",", q.y, ",", q.z, ") is ", n, + ". The norm of the quaternion you want to normalize should be greater than ", + std::numeric_limits::epsilon()); + } + return geometry_msgs::build().x(q.x / n).y(q.y / n).z(q.z / n).w( + q.w / n); +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__NORMALIZE_HPP_ diff --git a/common/math/geometry/include/geometry/quaternion/operator.hpp b/common/math/geometry/include/geometry/quaternion/operator.hpp index 624dd686852..ca124ed5e8d 100644 --- a/common/math/geometry/include/geometry/quaternion/operator.hpp +++ b/common/math/geometry/include/geometry/quaternion/operator.hpp @@ -16,6 +16,7 @@ #define GEOMETRY__QUATERNION__OPERATOR_HPP_ #include +#include #include namespace math @@ -64,6 +65,34 @@ auto operator*(const T & a, const U & b) return v; } +template < + typename T, typename U, + std::enable_if_t, IsLikeVector3>, std::nullptr_t> = + nullptr> +auto operator*(const T & a, const U & b) +{ + T b_quat; + b_quat.x = b.x; + b_quat.y = b.y; + b_quat.z = b.z; + b_quat.w = 0.0; + + T a_inv = a; + a_inv.x = -a.x; + a_inv.y = -a.y; + a_inv.z = -a.z; + a_inv.w = a.w; + + T result_quat = a * b_quat * a_inv; + + U rotated_vector; + rotated_vector.x = result_quat.x; + rotated_vector.y = result_quat.y; + rotated_vector.z = result_quat.z; + + return rotated_vector; +} + template < typename T, typename U, std::enable_if_t, IsLikeQuaternion>, std::nullptr_t> = diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 3c69df7d004..f3231594ffb 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -544,13 +545,13 @@ 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(); const auto desired_velocity = geometry_msgs::build() .x(twist_new.linear.x) .y(twist_new.linear.y) .z(twist_new.linear.z); auto displacement = desired_velocity * step_time; + displacement = math::geometry::normalize(pose_new.orientation) * displacement; auto adjustPositionAtLaneletBoundary = [&](double remaining_lanelet_length, const std::optional & next_lanelet_id) { @@ -572,6 +573,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( if (!canonicalized_entity_status->laneMatchingSucceed()) { pose_new.position += displacement; } else { + const auto current_lanelet_Id = canonicalized_entity_status->getLaneletId(); const auto lanelet_pose = canonicalized_entity_status->getLaneletPose().s; applyPitchToDisplacement(displacement, getLaneletPitch(current_lanelet_Id, lanelet_pose)); const double remaining_lanelet_length =