From 2ba428fe1499da2d49320c3863d3a53d47ebf757 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Thu, 23 Jan 2025 11:48:54 +0100 Subject: [PATCH] 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_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( estimated_next_canonicalized_lanelet_pose.value()) .lanelet_id; auto entity_status = static_cast(*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() .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(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(