Removing unnecessary changes in field_operator_application_for_autowa…

…re_universe Signed-off-by: Paweł Lech <[email protected]>
tier4 · Feb 22, 2024 · 9f1c8a3 · 9f1c8a3
1 parent f48075e
commit 9f1c8a3
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 37 deletions.
diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp
@@ -351,20 +351,18 @@ auto FieldOperatorApplicationFor<AutowareUniverse>::clearRoute() -> void
 
 auto FieldOperatorApplicationFor<AutowareUniverse>::engage() -> void
 {
-  if (!engaged() && !isDriving()) {
-    task_queue.delay([this]() {
-      waitForAutowareStateToBeDriving([this]() {
-        auto request = std::make_shared<tier4_external_api_msgs::srv::Engage::Request>();
-        request->engage = true;
-        try {
-          return requestEngage(request);
-        } catch (const decltype(requestEngage)::TimeoutError &) {
-          // ignore timeout error because this service is validated by Autoware state transition.
-          return;
-        }
-      });
+  task_queue.delay([this]() {
+    waitForAutowareStateToBeDriving([this]() {
+      auto request = std::make_shared<tier4_external_api_msgs::srv::Engage::Request>();
+      request->engage = true;
+      try {
+        return requestEngage(request);
+      } catch (const decltype(requestEngage)::TimeoutError &) {
+        // ignore timeout error because this service is validated by Autoware state transition.
+        return;
+      }
     });
-  }
+  });
 }
 
 auto FieldOperatorApplicationFor<AutowareUniverse>::engageable() const -> bool

diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp
@@ -114,32 +114,32 @@ void CppScenarioNode::spawnEgoEntity(
   }());
   api_.requestAssignRoute("ego", goal_lanelet_poses);
   using namespace std::chrono_literals;
-  std::atomic<bool> initialized(false);
-  auto initialize_thread = std::thread([&]() {
-    while (!api_.asFieldOperatorApplication("ego").engaged()) {
-      api_.updateFrame();
-      std::this_thread::sleep_for(std::chrono::duration<double>(1.0 / 20.0));
-    }
-    initialized.store(true);
-  });
-  std::atomic<bool> engaged(false);
-  auto engage_thread = std::thread([&]() {
-    while (!api_.asFieldOperatorApplication("ego").engaged()) {
-      if (!api_.asFieldOperatorApplication("ego").engageable()) {
-        api_.asFieldOperatorApplication("ego").engage();
-      } else {
-      }
-      std::this_thread::sleep_for(3000ms);
-    }
-    engaged.store(true);
-  });
-  while (!api_.asFieldOperatorApplication("ego").engaged() &&
-         !(initialized.load() && engaged.load())) {
-    // RCLCPP_INFO_STREAM(get_logger(), "Waiting for Autoware initialization...");
+  // std::atomic<bool> initialized(false);
+  // auto initialize_thread = std::thread([&]() {
+  //   while (!api_.asFieldOperatorApplication("ego").engaged()) {
+  //     api_.updateFrame();
+  //     std::this_thread::sleep_for(std::chrono::duration<double>(1.0 / 20.0));
+  //   }
+  //   initialized.store(true);
+  // });
+  // std::atomic<bool> engaged(false);
+  // auto engage_thread = std::thread([&]() {
+  while (!api_.asFieldOperatorApplication("ego").engaged()) {
+    if (api_.asFieldOperatorApplication("ego").engageable())
+      api_.asFieldOperatorApplication("ego").engage();
+    api_.updateFrame();
     std::this_thread::sleep_for(std::chrono::duration<double>(1.0 / 20.0));
+    // std::this_thread::sleep_for(1000ms);
   }
-  initialize_thread.join();
-  engage_thread.join();
+  // engaged.store(true);
+  // });
+  // while (!api_.asFieldOperatorApplication("ego").engaged() &&
+  //  !(initialized.load() && engaged.load())) {
+  // RCLCPP_INFO_STREAM(get_logger(), "Waiting for Autoware initialization...");
+  // std::this_thread::sleep_for(std::chrono::duration<double>(1.0 / 20.0));
+  // }
+  // initialize_thread.join();
+  // engage_thread.join();
 }
 
 void CppScenarioNode::checkConfiguration(const traffic_simulator::Configuration & configuration)