diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 09d3a588c5e..438f5b31620 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -528,46 +528,45 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   const auto buildUpdatedPose =
     [&include_crosswalk, &matching_distance](
       const std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> & status,
-      const geometry_msgs::msg::Twist & desired_twist, const double step_time,
-      const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
-    -> geometry_msgs::msg::Pose {
-    geometry_msgs::msg::Pose updated_pose;
-
-    // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s)
-    geometry_msgs::msg::Vector3 delta_rotation;
-    delta_rotation.z = desired_twist.angular.z * step_time;
-    const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
-    updated_pose.orientation = status->getMapPose().orientation * delta_quaternion;
-
-    // apply position change
-    /// @todo first determine global desired_velocity, calculate position change using it
-    // then pass the same global desired_velocity to moveTowardsLaneletPose()
-    const Eigen::Matrix3d rotation_matrix =
-      math::geometry::getRotationMatrix(updated_pose.orientation);
-    const Eigen::Vector3d translation = Eigen::Vector3d(
-      desired_twist.linear.x * step_time, desired_twist.linear.y * step_time,
-      desired_twist.linear.z * step_time);
-    const Eigen::Vector3d delta_position = rotation_matrix * translation;
-    updated_pose.position = status->getMapPose().position + delta_position;
-
-    // if it is the transition between lanelet pose: overwrite position to improve precision
-    if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
-      const auto estimated_next_canonicalized_lanelet_pose =
-        traffic_simulator::pose::toCanonicalizedLaneletPose(
-          updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance,
-          hdmap_utils_ptr);
-
-      // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same
-      if (estimated_next_canonicalized_lanelet_pose) {
-        const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose(
-          canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
-          desired_twist.linear, desired_velocity_is_global, step_time, hdmap_utils_ptr);
-        updated_pose.position =
-          traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position;
+      const geometry_msgs::msg::Twist & desired_twist, const double time_step,
+      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)
+      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
+      /// @todo first determine global desired_velocity, calculate position change using it
+      // then pass the same global desired_velocity to moveTowardsLaneletPose()
+      const Eigen::Matrix3d rotation_matrix =
+        math::geometry::getRotationMatrix(updated_pose.orientation);
+      const auto translation = Eigen::Vector3d(
+        desired_twist.linear.x * time_step, desired_twist.linear.y * time_step,
+        desired_twist.linear.z * time_step);
+      const Eigen::Vector3d delta_position = rotation_matrix * translation;
+      updated_pose.position = status->getMapPose().position + delta_position;
+
+      // if it is the transition between lanelet pose: overwrite position to improve precision
+      if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
+        const auto estimated_next_canonicalized_lanelet_pose =
+          traffic_simulator::pose::toCanonicalizedLaneletPose(
+            updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance,
+            hdmap_utils_ptr);
+
+        // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same
+        if (estimated_next_canonicalized_lanelet_pose) {
+          const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose(
+            canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
+            desired_twist.linear, desired_velocity_is_global, time_step, hdmap_utils_ptr);
+          updated_pose.position =
+            traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position;
+        }
       }
-    }
-    return updated_pose;
-  };
+      return updated_pose;
+    };
 
   const auto speed_planner =
     traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner(
@@ -576,8 +575,8 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
     target_speed, constraints, canonicalized_entity_status->getTwist(),
     canonicalized_entity_status->getAccel());
   const auto linear_jerk_new = std::get<2>(dynamics);
-  const auto accel_new = std::get<1>(dynamics);
-  const auto twist_new = std::get<0>(dynamics);
+  const auto & accel_new = std::get<1>(dynamics);
+  const auto & twist_new = std::get<0>(dynamics);
   const auto pose_new =
     buildUpdatedPose(canonicalized_entity_status, twist_new, step_time, hdmap_utils);