From ad6c16b9d47f74506c644832f343856a3bd66d3c Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Fri, 20 Dec 2024 16:14:48 +0100
Subject: [PATCH] ref(traffic_simulator): tidy up after moveTowardsLaneletPose
 development

---
 .../follow_polyline_trajectory_action.cpp                     | 1 -
 .../traffic_simulator/src/behavior/follow_trajectory.cpp      | 4 ++--
 simulation/traffic_simulator/src/utils/pose.cpp               | 2 +-
 3 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
index 07b48bae9c3..d0f65ea7c80 100644
--- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
+++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
@@ -68,7 +68,6 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
       not getInput<decltype(target_speed)>("target_speed", target_speed) or
       not polyline_trajectory) {
-    std::cout << "~~FollowPolylineTrajectoryAction" << std::endl;
     return BT::NodeStatus::FAILURE;
   } else if (std::isnan(canonicalized_entity_status->getTime())) {
     THROW_SIMULATION_ERROR(
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index f0da884b3b5..ef9cbd8ea18 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -558,8 +558,6 @@ auto makeUpdatedStatus(
     */
     auto updated_status = entity_status;
 
-    updated_status.lanelet_pose_valid = false;
-
     updated_status.pose.position += desired_velocity * step_time;
 
     updated_status.pose.orientation = [&]() {
@@ -619,6 +617,8 @@ auto makeUpdatedStatus(
 
     updated_status.time = entity_status.time + step_time;
 
+    updated_status.lanelet_pose_valid = false;
+
     return updated_status;
   }
 }
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 65609776db9..a1b55266d3e 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -155,6 +155,7 @@ auto moveTowardsLaneletPose(
   const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
   const auto next_lanelet_pose = static_cast<LaneletPose>(next_canonicalized_lanelet_pose);
 
+  // determine the displacement in the 2D lanelet coordinate system
   Eigen::Vector2d displacement;
   if (desired_velocity_is_global) {
     // transform desired (global) velocity to local velocity
@@ -164,7 +165,6 @@ auto moveTowardsLaneletPose(
       desired_velocity.x, desired_velocity.y, desired_velocity.z);
     const Eigen::Quaterniond quaternion(orientation.w, orientation.x, orientation.y, orientation.z);
     const Eigen::Vector3d local_velocity = quaternion.inverse() * global_velocity;
-    // determine the displacement in the 2D lanelet coordinate system
     displacement = Eigen::Rotation2Dd(lanelet_pose.rpy.z) *
                    Eigen::Vector2d(local_velocity.x(), local_velocity.y()) * step_time;
   } else {