first commit

src/bridges/mission_adapter_bridge.cpp

@@ -0,0 +1,239 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — cài đặt MissionAdapterBridge.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/bridges/mission_adapter_bridge.h>
+
+#include <utility>
+
+#include <mission_adapters/mission_manager.h>
+#include <robot/robot.h>
+
+namespace move_base2
+{
+
+MissionAdapterBridge::MissionAdapterBridge() = default;
+MissionAdapterBridge::~MissionAdapterBridge() = default;
+
+void MissionAdapterBridge::attach(mission_adapters::MissionManager* manager)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  manager_ = manager;
+}
+
+void MissionAdapterBridge::setCancelCallback(CancelCallback callback)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  cancel_callback_ = std::move(callback);
+}
+
+void MissionAdapterBridge::setRequestCallback(RequestCallback callback)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  request_callback_ = std::move(callback);
+}
+
+// ================================================================================================
+// Chuyển đổi
+// ================================================================================================
+
+NavigationRequest MissionAdapterBridge::toRequest(const mission_adapters::Mission& mission)
+{
+  NavigationRequest request;
+
+  request.mission_sequence_id = mission.id;
+  request.has_goal = mission.has_goal;
+  request.goal = mission.goal;
+
+  // Sai số để mặc định: quy ước của NavigationRequest là giá trị <= 0 nghĩa "dùng default của
+  // profile trong config". Mission layer không biết gì về sai số hình học nên không được đặt.
+  request.tolerance = GoalTolerance();
+
+  // Mission layer KHÔNG diễn giải actionType và không lọc gì (D8) — action đi qua nguyên vẹn, đúng
+  // thứ tự đã sắp theo sequenceId.
+  request.actions.reserve(mission.actions.size());
+  for (const auto& action : mission.actions)
+  {
+    request.actions.push_back(action.action);
+  }
+
+  // Mọi mission đều chạy profile position. `MissionType` chỉ nói mission đến TỪ ĐÂU (goal đơn hay
+  // order VDA5050), không nói robot phải di chuyển KIỂU gì — docking/go-straight/rotate là lựa chọn
+  // của người vận hành qua sáu entry point của contract host, không phải của mission layer.
+  request.profile = MotionProfile::kPosition;
+
+  return request;
+}
+
+// ================================================================================================
+// NavigationClient — gọi từ thread của MissionExecutor
+// ================================================================================================
+
+bool MissionAdapterBridge::dispatch(const std::shared_ptr<const mission_adapters::Mission>& mission)
+{
+  if (!mission)
+  {
+    robot::log_error("[move_base2] MissionAdapterBridge: dispatch(nullptr).\n");
+    return false;
+  }
+
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  if (!running_)
+  {
+    // Chưa start hoặc đã stop. Từ chối thay vì cất lại: mission layer phải biết chặng của nó không
+    // được nhận, chứ không phải chờ một kết quả sẽ không bao giờ tới.
+    robot::log_warning("[move_base2] MissionAdapterBridge: từ chối mission %llu — bridge chưa "
+                       "start.\n", static_cast<unsigned long long>(mission->id));
+    return false;
+  }
+
+  if (pending_)
+  {
+    // Không nên xảy ra: MissionManager chỉ giao chặng mới sau khi chặng cũ kết thúc. Đếm lại thay
+    // vì im lặng — một chặng biến mất trong khi fleet master vẫn chờ nó là lỗi rất khó truy.
+    ++dropped_requests_;
+    robot::log_warning("[move_base2] MissionAdapterBridge: mission %llu đè mission %llu chưa kịp "
+                       "đẩy xuống.\n", static_cast<unsigned long long>(mission->id),
+                       static_cast<unsigned long long>(pending_->id));
+  }
+
+  // Chỉ cất lại. Chuyển đổi và đẩy xuống navigation xảy ra ở pumpPendingRequest(), trên control
+  // thread — xem doc của lớp.
+  pending_ = mission;
+  return true;
+}
+
+void MissionAdapterBridge::cancelActive(mission_adapters::MissionId /*id*/)
+{
+  CancelCallback callback;
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    // Chặng đang chờ mà chưa kịp xuống navigation thì huỷ ngay tại đây: đẩy nó xuống rồi mới huỷ là
+    // cho robot nhúc nhích một cycle vì một chặng đã bị thu hồi.
+    pending_.reset();
+    callback = cancel_callback_;
+  }
+
+  // Gọi NGOÀI lock: callback đi vào control loop, không được chạy dưới mutex của bridge.
+  if (callback)
+  {
+    callback();
+  }
+}
+
+// ================================================================================================
+// Biên thread — chỉ control thread gọi
+// ================================================================================================
+
+bool MissionAdapterBridge::pumpPendingRequest()
+{
+  std::shared_ptr<const mission_adapters::Mission> mission;
+  RequestCallback callback;
+
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!pending_ || !request_callback_)
+    {
+      return false;
+    }
+    mission = pending_;
+    pending_.reset();
+    callback = request_callback_;
+  }
+
+  // Ngoài lock: callback đi thẳng vào ControlLoop::submit.
+  callback(toRequest(*mission));
+  return true;
+}
+
+// ================================================================================================
+// MissionPort — gọi từ control thread
+// ================================================================================================
+
+void MissionAdapterBridge::reportOutcome(std::uint64_t mission_sequence_id,
+                                         NavigationOutcome outcome)
+{
+  if (mission_sequence_id == mission_adapters::kInvalidMissionId)
+  {
+    // Goal trực tiếp từ contract host, không thuộc mission nào. Không có gì để báo.
+    return;
+  }
+
+  mission_adapters::MissionManager* manager = nullptr;
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    manager = manager_;
+  }
+
+  if (manager == nullptr)
+  {
+    return;
+  }
+
+  // Chỉ kSucceeded mới là "chặng xong". Ba kết cục còn lại đều là "chặng không hoàn thành", và
+  // chính sách hàng đợi (`clear_queue_on_failure`) nằm ở mission layer chứ không ở đây.
+  //
+  // kPreempted đáng chú ý: mission layer thường đã chuyển sang chặng khác rồi, nên
+  // onNavigationFailed sẽ trả false và không làm gì — đúng ý, chứ không phải bị bỏ sót.
+  const bool accepted = (outcome == NavigationOutcome::kSucceeded)
+                            ? manager->onNavigationDone(mission_sequence_id)
+                            : manager->onNavigationFailed(mission_sequence_id);
+
+  if (!accepted)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    ++stale_outcomes_;
+  }
+}
+
+bool MissionAdapterBridge::hasActiveMission() const
+{
+  mission_adapters::MissionManager* manager = nullptr;
+  bool has_pending = false;
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    manager = manager_;
+    has_pending = static_cast<bool>(pending_);
+  }
+
+  if (has_pending)
+  {
+    return true;
+  }
+  return manager != nullptr && manager->hasMission();
+}
+
+void MissionAdapterBridge::start()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  running_ = true;
+}
+
+void MissionAdapterBridge::stop()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  running_ = false;
+  // Bỏ chặng đang chờ: nó sẽ không bao giờ được chạy, và giữ lại chỉ để nó chạy sau một lần start()
+  // sau này là hành vi không ai mong đợi.
+  pending_.reset();
+}
+
+std::size_t MissionAdapterBridge::droppedRequests() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return dropped_requests_;
+}
+
+std::size_t MissionAdapterBridge::staleOutcomes() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return stale_outcomes_;
+}
+
+}  // namespace move_base2