ref(traffic_simulator): tidy up after moveTowardsLaneletPose development

tier4 · Dec 20, 2024 · ad6c16b · ad6c16b
1 parent b2074d4
commit ad6c16b
Show file tree

Hide file tree

Showing 3 changed files with 3 additions and 4 deletions.
diff --git a/...ee_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/...ee_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
@@ -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
@@ -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 {