ref(traffic_simulator) Refactor updateEntityPositionForLaneletTransition

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);
         }

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,

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);
       }

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(