230 lines
7.7 KiB
C++
230 lines
7.7 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* move_base2 — dựng toàn bộ thành phần runtime thật từ config và TF.
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
#ifndef MOVE_BASE2_NAVIGATION_RUNTIME_H_
|
|
#define MOVE_BASE2_NAVIGATION_RUNTIME_H_
|
|
|
|
#include <memory>
|
|
#include <string>
|
|
|
|
#include <robot_costmap_2d/costmap_2d_robot.h>
|
|
|
|
#include <move_base2/bridges/mission_adapter_bridge.h>
|
|
#include <move_base2/config/move_base2_config.h>
|
|
#include <move_base2/control_loop.h>
|
|
#include <move_base2/io/costmap_exporter.h>
|
|
#include <move_base2/ports/clock_port.h>
|
|
#include <move_base2/ports/pose_port.h>
|
|
#include <move_base2/runners/action_runner.h>
|
|
#include <move_base2/runners/controller_runner.h>
|
|
#include <move_base2/runners/planner_runner.h>
|
|
#include <move_base2/runners/recovery_runner.h>
|
|
|
|
namespace tf3
|
|
{
|
|
class BufferCore;
|
|
}
|
|
|
|
namespace move_base2
|
|
{
|
|
|
|
/**
|
|
* @class CostmapPosePort
|
|
* @brief Cổng pose lấy từ costmap.
|
|
*
|
|
* `Costmap2DROBOT::getRobotPose` đã gói sẵn việc tra TF kèm `transform_tolerance` và trả false khi
|
|
* transform quá cũ — đúng ngữ nghĩa mà @ref PosePort cần, nên không có gì phải viết lại.
|
|
*
|
|
* @note Con trỏ costmap là **non-owning**. Lấy lại mỗi lần dùng chứ không cache `Costmap2D*` bên
|
|
* trong: con trỏ costmap có thể bị thay.
|
|
*/
|
|
class CostmapPosePort final : public PosePort
|
|
{
|
|
public:
|
|
explicit CostmapPosePort(robot_costmap_2d::Costmap2DROBOT* costmap = nullptr) : costmap_(costmap)
|
|
{
|
|
}
|
|
|
|
void setCostmap(robot_costmap_2d::Costmap2DROBOT* costmap)
|
|
{
|
|
costmap_ = costmap;
|
|
}
|
|
|
|
bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override
|
|
{
|
|
if (costmap_ == nullptr)
|
|
{
|
|
return false;
|
|
}
|
|
return costmap_->getRobotPose(pose);
|
|
}
|
|
|
|
private:
|
|
robot_costmap_2d::Costmap2DROBOT* costmap_;
|
|
};
|
|
|
|
/**
|
|
* @class NavigationRuntime
|
|
* @brief Sở hữu và dựng mọi thành phần thật của runtime: costmap, runner, bridge.
|
|
*
|
|
* Tách khỏi @ref NavigationServer để chỗ **lắp ráp** không lẫn với chỗ **hiện thực contract host**.
|
|
* `NavigationServer` vẫn nhận cổng qua `configureLoop`, nên nó kiểm được bằng cổng giả y như trước;
|
|
* lớp này chỉ là một cách dựng bộ cổng đó từ config thật.
|
|
*
|
|
* ## Thứ tự dựng là bắt buộc, không phải tuỳ ý
|
|
*
|
|
* `state_machine.recovery_behavior_count` **không đến từ YAML** — nó là số behavior
|
|
* `RecoveryRunner` nạp được **thật**. Đọc từ config thì một behavior hỏng vẫn khiến state machine
|
|
* tin là còn đường phục hồi, và lỗi đầu tiên sẽ dẫn thẳng tới `ABORTED` mà không ai hiểu vì sao.
|
|
* Trình tự bắt buộc, được @ref build tuân thủ:
|
|
*
|
|
* `MoveBase2Config::load()` → `RecoveryRunner::configure()` → gán `recovery_behavior_count`
|
|
* → `MoveBase2Config::validate()`
|
|
*
|
|
* ## Costmap
|
|
*
|
|
* Hai costmap được dựng ở đây và **chỉ `start()` sau khi mọi thứ khác đã sẵn sàng**: `start()` cho
|
|
* chạy thread cập nhật của costmap, và một thread cập nhật chạy trong khi planner chưa nạp xong là
|
|
* cửa sổ để mọi thứ chạm vào nhau ở trạng thái nửa vời.
|
|
*/
|
|
class NavigationRuntime
|
|
{
|
|
public:
|
|
NavigationRuntime();
|
|
~NavigationRuntime();
|
|
|
|
NavigationRuntime(const NavigationRuntime&) = delete;
|
|
NavigationRuntime& operator=(const NavigationRuntime&) = delete;
|
|
|
|
/**
|
|
* @brief Pha 1: đọc config và dựng hai costmap. **Chưa nạp plugin nào.**
|
|
*
|
|
* Tách khỏi @ref buildRunners là bắt buộc, không phải để cho gọn. `SBPLLatticePlanner::initialize`
|
|
* **chặn** cho tới khi costmap có kích thước khác 0 (`sbpl_lattice_planner.cpp:132`), mà kích
|
|
* thước đó chỉ được đặt khi static map tới `StaticLayer`. Nếu nạp planner ngay trong một pha thì
|
|
* planner chờ map, còn map thì chờ pha đó xong để được đẩy vào — khoá chết, và SBPL `exit(1)` sau
|
|
* 2 giây, giết cả tiến trình.
|
|
*
|
|
* Trình tự đúng, giống hệt bản cũ (`move_base.cpp:260` dựng costmap → `:287` đẩy map → `:306` nạp
|
|
* planner): dựng costmap → gắn vào SensorGateway và phát lại static map → rồi mới nạp plugin.
|
|
*
|
|
* @param tf Buffer TF do host cấp. Bắt buộc khác null; phải sống lâu hơn object này.
|
|
* @param[out] error Lý do thất bại; chỉ ghi khi hàm trả false.
|
|
*/
|
|
bool buildCostmaps(const std::shared_ptr<tf3::BufferCore>& tf, std::string& error);
|
|
|
|
/**
|
|
* @brief Pha 2: nạp planner, controller, recovery, action rồi kiểm cấu hình.
|
|
*
|
|
* Chỉ gọi sau khi static map đã được đẩy vào costmap — xem @ref buildCostmaps.
|
|
*
|
|
* Thất bại ở bất kỳ bước nào cũng để runtime ở trạng thái **chưa dựng** (@ref built false), chứ
|
|
* không phải nửa dựng nửa không.
|
|
*/
|
|
bool buildRunners(std::string& error);
|
|
|
|
/// @brief Hai costmap đã dựng xong chưa (pha 1).
|
|
bool costmapsReady() const
|
|
{
|
|
return global_costmap_ != nullptr && local_costmap_ != nullptr;
|
|
}
|
|
|
|
bool built() const
|
|
{
|
|
return built_;
|
|
}
|
|
|
|
/// @brief Cho costmap bắt đầu cập nhật. Gọi sau khi control loop đã được cấu hình xong.
|
|
void start();
|
|
|
|
/// @brief Dừng cập nhật costmap.
|
|
void stop();
|
|
|
|
/// @brief Bộ cổng để bơm vào @ref ControlLoop. Rỗng nếu chưa @ref build.
|
|
ControlLoopDeps deps();
|
|
|
|
const MoveBase2Config& config() const
|
|
{
|
|
return config_;
|
|
}
|
|
|
|
robot_costmap_2d::Costmap2DROBOT* globalCostmap()
|
|
{
|
|
return global_costmap_.get();
|
|
}
|
|
|
|
robot_costmap_2d::Costmap2DROBOT* localCostmap()
|
|
{
|
|
return local_costmap_.get();
|
|
}
|
|
|
|
MissionAdapterBridge& mission()
|
|
{
|
|
return mission_;
|
|
}
|
|
|
|
PlannerRunner& planner()
|
|
{
|
|
return planner_;
|
|
}
|
|
|
|
ControllerRunner& controller()
|
|
{
|
|
return controller_;
|
|
}
|
|
|
|
/// @brief Bộ kết xuất costmap cho host publish lên rviz.
|
|
CostmapExporter& globalExporter()
|
|
{
|
|
return global_exporter_;
|
|
}
|
|
|
|
CostmapExporter& localExporter()
|
|
{
|
|
return local_exporter_;
|
|
}
|
|
|
|
private:
|
|
MoveBase2Config config_;
|
|
bool built_ = false;
|
|
|
|
std::shared_ptr<tf3::BufferCore> tf_;
|
|
|
|
// Costmap phải được khai TRƯỚC các runner: runner giữ con trỏ tới chúng, nên chúng phải bị huỷ
|
|
// SAU. Thứ tự khai báo thành viên chính là thứ tự huỷ ngược.
|
|
std::unique_ptr<robot_costmap_2d::Costmap2DROBOT> global_costmap_;
|
|
std::unique_ptr<robot_costmap_2d::Costmap2DROBOT> local_costmap_;
|
|
|
|
SystemClock clock_;
|
|
|
|
/**
|
|
* HAI nguồn pose, khác FRAME — không gộp được.
|
|
*
|
|
* `Costmap2DROBOT::getRobotPose` trả pose trong **global frame của chính costmap đó**: costmap
|
|
* global dùng `map`, costmap local dùng `odom`. Global planner lập plan trên lưới `map` nên start
|
|
* phải ở `map`; local planner và recovery làm việc trên lưới `odom` nên pose phải ở `odom`.
|
|
*
|
|
* Dùng nhầm nguồn không gây lỗi nào nhìn thấy được — chỉ là start của planner nằm ở gốc odom thay
|
|
* vì vị trí thật, và planner báo "failed to set start state". Đã xảy ra một lần.
|
|
*/
|
|
CostmapPosePort global_pose_; ///< frame `map` — planner, chống quẩn, pose báo về host.
|
|
CostmapPosePort local_pose_; ///< frame `odom` — controller và recovery.
|
|
CostmapExporter global_exporter_;
|
|
CostmapExporter local_exporter_;
|
|
|
|
PlannerRunner planner_;
|
|
ControllerRunner controller_;
|
|
RecoveryRunner recovery_;
|
|
ActionRunner action_;
|
|
MissionAdapterBridge mission_;
|
|
};
|
|
|
|
} // namespace move_base2
|
|
|
|
#endif // MOVE_BASE2_NAVIGATION_RUNTIME_H_
|