fix: respawn ego mock test

Signed-off-by: satoshi-ota <[email protected]>
tier4 · Dec 27, 2024 · bac3ffd · bac3ffd
1 parent bbf40d2
commit bac3ffd
Showing 1 changed file with 26 additions and 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
@@ -39,17 +39,39 @@ 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();
   }
 
 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