From 37d902b5bf52ec1117b816e96d986e4a952b305d Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 17 Dec 2024 16:51:52 +0100 Subject: [PATCH] fix(traffic_simulator): fix follow_trajectory_action issue, add orientation to distance calc, remove toCanonicalizedLaneletPose(point...) because it can cause a another issues --- .../quaternion/direction_to_quaternion.hpp | 46 +++++++++++++++++++ .../include/traffic_simulator/utils/pose.hpp | 5 -- .../src/behavior/follow_trajectory.cpp | 22 +++++---- .../traffic_simulator/src/utils/pose.cpp | 11 ----- 4 files changed, 59 insertions(+), 25 deletions(-) create mode 100644 common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp diff --git a/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp b/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp new file mode 100644 index 00000000000..7a31f056a7f --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp @@ -0,0 +1,46 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_ +#define GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_ + +#include +#include +#include +#include +#include + +namespace math +{ +namespace geometry +{ +auto convertDirectionToQuaternion(const double dx, const double dy, const double dz) +{ + const auto euler_angles = geometry_msgs::build() + .x(0.0) + .y(std::atan2(-dz, std::hypot(dx, dy))) + .z(std::atan2(dy, dx)); + return math::geometry::convertEulerAngleToQuaternion(euler_angles); +} + +template < + typename T, std::enable_if_t>, std::nullptr_t> = nullptr> +auto convertDirectionToQuaternion(const T & v) +{ + return convertDirectionToQuaternion(v.x, v.y, v.z); +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 92371213c72..fa85c4f26a4 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -45,11 +45,6 @@ auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const bool include_crosswalk) -> std::optional; -auto toCanonicalizedLaneletPose( - const geometry_msgs::msg::Point & map_point, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk, - const double matching_distance) -> std::optional; - auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk, diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 92e1ad14b46..2e1df5ddadd 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include #include @@ -68,11 +68,20 @@ auto makeUpdatedStatus( auto distance_along_lanelet = [&](const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double { + const auto quaternion = math::geometry::convertDirectionToQuaternion( + geometry_msgs::build() + .x(to.x - from.x) + .y(to.y - from.y) + .z(to.z - from.z)); + const auto from_pose = + geometry_msgs::build().position(from).orientation(quaternion); + const auto to_pose = + geometry_msgs::build().position(to).orientation(quaternion); if (const auto from_lanelet_pose = pose::toCanonicalizedLaneletPose( - from, entity_status.bounding_box, false, matching_distance); + from_pose, entity_status.bounding_box, false, matching_distance); from_lanelet_pose) { if (const auto to_lanelet_pose = pose::toCanonicalizedLaneletPose( - to, entity_status.bounding_box, false, matching_distance); + to_pose, entity_status.bounding_box, false, matching_distance); to_lanelet_pose) { if (const auto distance = hdmap_utils->getLongitudinalDistance( static_cast(from_lanelet_pose.value()), @@ -561,12 +570,7 @@ auto makeUpdatedStatus( return entity_status.pose.orientation; } else { // if there is a designed_velocity vector, set the orientation in the direction of it - const geometry_msgs::msg::Vector3 direction = - geometry_msgs::build() - .x(0.0) - .y(std::atan2(-desired_velocity.z, std::hypot(desired_velocity.x, desired_velocity.y))) - .z(std::atan2(desired_velocity.y, desired_velocity.x)); - return math::geometry::convertEulerAngleToQuaternion(direction); + return math::geometry::convertDirectionToQuaternion(desired_velocity); } }(); diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index ace0a5a7b28..72e3b7dd67b 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -134,17 +134,6 @@ auto toCanonicalizedLaneletPose( } } -auto toCanonicalizedLaneletPose( - const geometry_msgs::msg::Point & map_point, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk, - const double matching_distance) -> std::optional -{ - return toCanonicalizedLaneletPose( - geometry_msgs::build().position(map_point).orientation( - geometry_msgs::build().x(0).y(0).z(0).w(1)), - bounding_box, include_crosswalk, matching_distance); -} - auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box,