240 lines
7.9 KiB
C++
240 lines
7.9 KiB
C++
/*********************************************************************
|
|
*
|
|
* 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
|