From 72eea6f3580f3bd6b53681e61b737f72ae9e1c17 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 19 Dec 2024 02:28:55 +0100 Subject: [PATCH] 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_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_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_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; }