177 lines
6.8 KiB
C++
177 lines
6.8 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* move_base2 — hiện thực PlannerPort bằng plugin robot_nav_core::BaseGlobalPlanner, chạy trên
|
|
* thread riêng.
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
#ifndef MOVE_BASE2_RUNNERS_PLANNER_RUNNER_H_
|
|
#define MOVE_BASE2_RUNNERS_PLANNER_RUNNER_H_
|
|
|
|
#include <condition_variable>
|
|
#include <cstddef>
|
|
#include <cstdint>
|
|
#include <functional>
|
|
#include <map>
|
|
#include <memory>
|
|
#include <mutex>
|
|
#include <string>
|
|
#include <thread>
|
|
#include <vector>
|
|
|
|
#include <robot/node_handle.h>
|
|
#include <robot_nav_core/base_global_planner.h>
|
|
|
|
#include <move_base2/ports/planner_port.h>
|
|
|
|
namespace robot_costmap_2d
|
|
{
|
|
class Costmap2DROBOT;
|
|
}
|
|
|
|
namespace move_base2
|
|
{
|
|
|
|
/**
|
|
* @class PlannerRunner
|
|
* @brief Nạp và chạy global planner thế hệ 1 (`robot_nav_core::BaseGlobalPlanner`) qua Boost.DLL,
|
|
* trên một thread riêng.
|
|
*
|
|
* Đây là chỗ duy nhất trong gói biết tới `robot_nav_core`. Lõi quyết định chỉ thấy @ref PlannerPort.
|
|
*
|
|
* ## Vì sao có thread riêng
|
|
*
|
|
* Global planner nặng mất hàng trăm ms; control loop chạy 20 Hz và là thread duy nhất phát
|
|
* `cmd_vel`. Lập plan tại chỗ nghĩa là mỗi lần lập lại plan là ngần ấy thời gian robot chạy bằng
|
|
* lệnh cũ không ai giám sát.
|
|
*
|
|
* ## Bàn giao plan bằng hoán vị, không copy
|
|
*
|
|
* Một plan toàn cục có thể vài nghìn pose. Copy nó mỗi lượt là cấp phát lớn trên đường nóng. Ở đây
|
|
* có **ba** vector luân chuyển bằng `swap`, đúng mô hình triple buffer của bản cũ nhưng bằng giá
|
|
* trị thay vì con trỏ thô:
|
|
*
|
|
* ```
|
|
* planning_ thread planner ghi vào
|
|
* handoff_ hộp thư, đổi dưới mutex
|
|
* (của bên gọi) pollPlan hoán vị với handoff_ -> vector cũ của bên gọi quay lại làm hộp thư
|
|
* ```
|
|
*
|
|
* Mutex chỉ bị giữ trong lúc đổi vector, không bao giờ trong lúc plugin đang tính.
|
|
*
|
|
* ## Nhãn yêu cầu (tag)
|
|
*
|
|
* Lượt lập plan sống lâu hơn cái goal sinh ra nó. Mỗi lượt mang một nhãn do bên gọi cấp; bên gọi so
|
|
* nhãn và vứt kết quả không khớp. Không có nó thì một plan tới goal đã bị huỷ vẫn được bám theo.
|
|
*
|
|
* ## Giữ instance sống bằng cách nào
|
|
*
|
|
* `boost::dll::import_alias` trả về factory **giữ tham chiếu tới thư viện đã nạp**. Vứt factory đi
|
|
* trong khi instance nó tạo ra còn sống là để `.so` bị unload dưới chân object. Mỗi entry vì thế
|
|
* giữ **cả hai**. Cache cũng **không bao giờ xoá** entry — nhờ đó con trỏ planner mà thread đang
|
|
* dùng vẫn hợp lệ kể cả khi @ref swapPlanner đổi sang planner khác giữa chừng.
|
|
*
|
|
* @note Các hàm public gọi từ control thread. Thread nội bộ chỉ chạm dữ liệu dưới mutex.
|
|
* @warning Destructor **chờ** lượt đang chạy kết thúc: plugin là hộp đen, không có đường cắt ngang.
|
|
*/
|
|
class PlannerRunner : public PlannerPort
|
|
{
|
|
public:
|
|
PlannerRunner();
|
|
~PlannerRunner() override;
|
|
|
|
PlannerRunner(const PlannerRunner&) = delete;
|
|
PlannerRunner& operator=(const PlannerRunner&) = delete;
|
|
|
|
/**
|
|
* @brief Nạp cấu hình, planner khởi đầu, và khởi động thread.
|
|
*
|
|
* @param nh NodeHandle để tra `library_path`. Được **sao chép** vì @ref swapPlanner
|
|
* cần tra lại lúc chạy.
|
|
* @param costmap Costmap global truyền cho `BaseGlobalPlanner::initialize`. **Non-owning**,
|
|
* bắt buộc khác null, phải sống lâu hơn object này.
|
|
* @param initial_planner Alias plugin nạp ngay. Chuỗi rỗng = chờ @ref swapPlanner.
|
|
* @param[out] error Lý do thất bại; chỉ ghi khi hàm trả false.
|
|
*/
|
|
bool configure(const robot::NodeHandle& nh, robot_costmap_2d::Costmap2DROBOT* costmap,
|
|
const std::string& initial_planner, std::string& error);
|
|
|
|
bool configured() const
|
|
{
|
|
return configured_;
|
|
}
|
|
|
|
// ================================================================================================
|
|
// PlannerPort
|
|
// ================================================================================================
|
|
|
|
bool swapPlanner(const std::string& planner_name) override;
|
|
|
|
bool startPlan(const robot_geometry_msgs::PoseStamped& start,
|
|
const robot_geometry_msgs::PoseStamped& goal,
|
|
const robot_protocol_msgs::Order* order, std::uint64_t tag) override;
|
|
|
|
bool isPlanning() const override;
|
|
|
|
bool pollPlan(PlanResult& result) override;
|
|
|
|
void cancelPlan() override;
|
|
|
|
std::string activePlanner() const override;
|
|
|
|
/// @brief Số plugin đã nạp và còn giữ trong cache — để kiểm việc dùng lại, không phải để log.
|
|
std::size_t loadedCount() const
|
|
{
|
|
return planners_.size();
|
|
}
|
|
|
|
private:
|
|
/// @brief Một plugin đã nạp: factory phải sống cùng instance, xem doc của lớp.
|
|
struct Loaded
|
|
{
|
|
std::function<robot_nav_core::BaseGlobalPlanner::Ptr()> factory;
|
|
robot_nav_core::BaseGlobalPlanner::Ptr instance;
|
|
};
|
|
|
|
/// @brief Nạp @p name nếu chưa có trong cache. @return nullptr khi thất bại (đã log lý do).
|
|
robot_nav_core::BaseGlobalPlanner* acquire(const std::string& name);
|
|
|
|
/// @brief Thân thread: ngủ tới khi có yêu cầu, chạy plugin, đặt kết quả vào hộp thư.
|
|
void threadBody();
|
|
|
|
// --- Chỉ control thread chạm ------------------------------------------------------------------
|
|
robot::NodeHandle nh_;
|
|
robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr;
|
|
bool configured_ = false;
|
|
std::map<std::string, Loaded> planners_;
|
|
std::string active_name_;
|
|
|
|
// --- Chia sẻ giữa hai thread, bảo vệ bởi mutex_ ------------------------------------------------
|
|
mutable std::mutex mutex_;
|
|
std::condition_variable cv_;
|
|
std::thread thread_;
|
|
|
|
bool shutdown_ = false;
|
|
bool pending_ = false; ///< Có yêu cầu chờ thread nhận.
|
|
bool running_ = false; ///< Thread đang chạy plugin.
|
|
bool discard_ = false; ///< Lượt đang chạy đã bị huỷ — vứt kết quả khi nó về.
|
|
bool has_result_ = false;
|
|
|
|
robot_nav_core::BaseGlobalPlanner* active_ = nullptr; ///< Non-owning, trỏ vào @ref planners_.
|
|
robot_geometry_msgs::PoseStamped request_start_;
|
|
robot_geometry_msgs::PoseStamped request_goal_;
|
|
std::shared_ptr<robot_protocol_msgs::Order> request_order_;
|
|
std::uint64_t request_tag_ = 0;
|
|
|
|
std::vector<robot_geometry_msgs::PoseStamped> planning_; ///< Thread ghi vào.
|
|
std::vector<robot_geometry_msgs::PoseStamped> handoff_; ///< Hộp thư.
|
|
std::uint64_t result_tag_ = 0;
|
|
bool result_ok_ = false;
|
|
};
|
|
|
|
} // namespace move_base2
|
|
|
|
#endif // MOVE_BASE2_RUNNERS_PLANNER_RUNNER_H_
|