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(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( + 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( + 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::SharedPtr new_position_subscriber; + + bool has_respawned; }; } // namespace cpp_mock_scenarios