From bac3ffd9d4b25f2fcb9cf9b392b89875d3bc0668 Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Sun, 22 Dec 2024 18:59:27 +0900
Subject: [PATCH] fix: respawn ego mock test

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../src/respawn_ego/respawn_ego.cpp           | 28 +++++++++++++++++--
 1 file changed, 26 insertions(+), 2 deletions(-)

diff --git a/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp
index f1db7fd4e06..6c5c109f49b 100644
--- a/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp
+++ b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp
@@ -39,7 +39,8 @@ class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode
         goal_msg.header.frame_id = "map";
         goal_msg.pose = static_cast<geometry_msgs::msg::Pose>(goal_pose);
         api_.respawn("ego", message, goal_msg);
-      })}
+      })},
+    has_respawned{false}
   {
     start();
   }
@@ -47,9 +48,30 @@ class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode
 private:
   void onUpdate() override
   {
-    if (api_.getCurrentTime() >= 30) {
+    if (api_.getCurrentTime() >= 60) {
+      stop(cpp_mock_scenarios::Result::FAILURE);
+    }
+
+    if (api_.isInLanelet("ego", 34564, 0.1)) {
       stop(cpp_mock_scenarios::Result::SUCCESS);
     }
+
+    if (api_.getCurrentTime() >= 10 && !has_respawned) {
+      geometry_msgs::msg::PoseWithCovarianceStamped ego_pose;
+      ego_pose.header.frame_id = "map";
+      ego_pose.pose.pose = static_cast<geometry_msgs::msg::Pose>(
+        traffic_simulator::helper::constructCanonicalizedLaneletPose(
+          34576, 10.0, 0.0, api_.getHdmapUtils()));
+
+      geometry_msgs::msg::PoseStamped goal_pose;
+      goal_pose.header.frame_id = "map";
+      goal_pose.pose = static_cast<geometry_msgs::msg::Pose>(
+        traffic_simulator::helper::constructCanonicalizedLaneletPose(
+          34564, 10.0, 0.0, api_.getHdmapUtils()));
+      api_.respawn("ego", ego_pose, goal_pose);
+
+      has_respawned = true;
+    }
   }
 
   void onInitialize() override
@@ -65,6 +87,8 @@ class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode
 
   const rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     new_position_subscriber;
+
+  bool has_respawned;
 };
 }  // namespace cpp_mock_scenarios