177 lines
6.6 KiB
Markdown
177 lines
6.6 KiB
Markdown
# Viết một nguồn mission mới
|
|
|
|
Nguồn mission là plugin duy nhất của gói này. Thêm một loại nguồn (REST, fleet manager riêng,
|
|
teach-pendant, file kịch bản...) không cần sửa dòng nào trong `src/`.
|
|
|
|
Việc **thực thi action** không thuộc đây: adapter chỉ chuyển dữ liệu, navigation runtime mới là nơi
|
|
chạy action.
|
|
|
|
## 1. Chọn tên schema
|
|
|
|
Schema là chuỗi định danh loại payload, và là thứ core dùng để định tuyến. Đặt tên theo miền dữ
|
|
liệu, không theo tên hãng hay giao thức truyền tải:
|
|
|
|
```
|
|
vda5050.order # đã có
|
|
geometry.pose_stamped # đã có
|
|
json.pick_and_place # ví dụ nguồn mới
|
|
```
|
|
|
|
Hai adapter khai cùng một schema sẽ bị registry từ chối — định tuyến khi đó phụ thuộc thứ tự nạp,
|
|
tức phụ thuộc vào thứ tự dòng trong file YAML.
|
|
|
|
## 2. Hiện thực `MissionSourceAdapter`
|
|
|
|
```cpp
|
|
#include <boost/dll/alias.hpp>
|
|
#include <mission_adapters/adapter.h>
|
|
|
|
namespace mission_plugins
|
|
{
|
|
|
|
class PickAndPlaceAdapter : public mission_adapters::MissionSourceAdapter
|
|
{
|
|
public:
|
|
static mission_adapters::MissionSourceAdapter::Ptr create()
|
|
{
|
|
return std::make_shared<PickAndPlaceAdapter>();
|
|
}
|
|
|
|
bool configure(const std::string& name, robot::NodeHandle& nh) override
|
|
{
|
|
// Param riêng nằm trong namespace tên instance.
|
|
nh.getParam(name + "/global_frame", global_frame_, std::string("map"));
|
|
return !global_frame_.empty();
|
|
}
|
|
|
|
std::string schema() const override { return "json.pick_and_place"; }
|
|
|
|
bool validate(const mission_adapters::MissionRequest& request,
|
|
std::string& reason) const override
|
|
{
|
|
if (request.raw_payload.empty())
|
|
{
|
|
reason = "payload rỗng";
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
mission_adapters::ConversionResult
|
|
convert(const mission_adapters::MissionRequest& request) override
|
|
{
|
|
mission_adapters::ConversionResult result;
|
|
// ... parse request.raw_payload, dựng mission ...
|
|
return result;
|
|
}
|
|
|
|
private:
|
|
std::string global_frame_ = "map";
|
|
};
|
|
|
|
} // namespace mission_plugins
|
|
|
|
BOOST_DLL_ALIAS(mission_plugins::PickAndPlaceAdapter::create, PickAndPlaceAdapter)
|
|
```
|
|
|
|
Bốn quy tắc bắt buộc:
|
|
|
|
- **State là member, không phải `static` local.** Một tiến trình có thể chạy nhiều instance; state
|
|
static sẽ nối chúng lại và order của robot này ảnh hưởng robot kia.
|
|
- **`validate()` chặn dữ liệu hỏng trước khi nó tới navigation**: NaN/Inf, quaternion không chuẩn
|
|
hoá được, payload thiếu trường. Trả `false` kèm `reason` cụ thể — chuỗi đó đi thẳng vào log.
|
|
- **`convert()` trả rỗng nghĩa là "không có việc", KHÔNG phải lỗi và KHÔNG phải lệnh huỷ.** Core sẽ
|
|
bỏ qua và giữ nguyên hàng đợi đang chạy. Đây là bất biến A1: một order lỗi từ fleet manager không
|
|
được âm thầm xoá order đang chạy.
|
|
- **Mission `has_goal == false` phải có ít nhất một action.** Mission không goal và cũng không
|
|
action là chặng không có việc gì để làm, và nó sẽ không bao giờ báo kết quả về — mission layer kẹt
|
|
`RUNNING` vĩnh viễn. Core kiểm lại điều này và bỏ cả lô nếu phát hiện.
|
|
|
|
### `SubmitMode` — thay hay nối tiếp
|
|
|
|
`ConversionResult::mode` quyết định mission mới quan hệ thế nào với hàng đợi đang có:
|
|
|
|
| mode | Ý nghĩa | Dùng khi |
|
|
|---|---|---|
|
|
| `kReplace` (mặc định) | Thay toàn bộ hàng đợi, chặng đang chạy bị preempt và được bảo dừng | Yêu cầu mới, độc lập với việc đang làm |
|
|
| `kAppend` | Chỉ thêm vào cuối hàng đợi, không đụng chặng đang chạy | Phần nối tiếp của chính yêu cầu đang chạy |
|
|
|
|
`kAppend` là hình dạng của VDA5050 order update. Trả `kReplace` cho một bản cập nhật nghĩa là mỗi
|
|
lần fleet manager release thêm horizon, robot lại huỷ và chạy lại chặng đang đi.
|
|
|
|
## 3. Build thành `.so` riêng
|
|
|
|
```cmake
|
|
add_library(mission_adapters_pick_and_place SHARED plugins/pick_and_place_adapter.cpp)
|
|
|
|
target_compile_options(mission_adapters_pick_and_place PRIVATE -Wall -Wextra)
|
|
set_target_properties(mission_adapters_pick_and_place PROPERTIES POSITION_INDEPENDENT_CODE ON)
|
|
|
|
target_link_libraries(mission_adapters_pick_and_place
|
|
PUBLIC mission_adapters
|
|
PRIVATE Boost::boost Threads::Threads
|
|
)
|
|
|
|
install(TARGETS mission_adapters_pick_and_place
|
|
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
|
)
|
|
```
|
|
|
|
Thêm dependency cho plugin thì phải cập nhật **đủ bốn chỗ**: `CMakeLists.txt`, `package.xml`,
|
|
install rule, và file YAML.
|
|
|
|
## 4. Khai báo trong YAML — **đừng quên `library_path`**
|
|
|
|
Trong `pnkx_nav_core/config/mission_adapters_params.yaml`:
|
|
|
|
```yaml
|
|
mission_adapters:
|
|
mission_sources:
|
|
- {name: pick_src, type: PickAndPlaceAdapter} # type = tên symbol trong BOOST_DLL_ALIAS
|
|
|
|
pick_src:
|
|
global_frame: map # param riêng của instance, đọc trong configure()
|
|
|
|
PickAndPlaceAdapter:
|
|
library_path: libmission_adapters_pick_and_place # BẮT BUỘC
|
|
```
|
|
|
|
Ba tên phải khớp nhau:
|
|
|
|
| Nơi | Giá trị |
|
|
|---|---|
|
|
| `BOOST_DLL_ALIAS(..., PickAndPlaceAdapter)` | tên symbol |
|
|
| `mission_sources[].type` | **cùng** tên symbol |
|
|
| `<type>/library_path` | tên file `.so` (không cần đuôi) |
|
|
|
|
**Thiếu `library_path` là lỗi phổ biến nhất**: plugin build sạch, `.so` nằm đúng chỗ, nhưng runtime
|
|
báo "không tìm thấy". Registry sẽ nêu đích danh khoá bị thiếu trong log.
|
|
|
|
Tên không có đuôi `.so` được resolve qua `PNKX_NAV_CORE_LIBRARY_PATH`, `devel/lib`, rồi
|
|
`LD_LIBRARY_PATH` — nên **phải `source devel/setup.bash`** trước khi chạy, nếu không plugin sẽ không
|
|
được tìm thấy dù file có thật.
|
|
|
|
## 5. Kiểm chứng
|
|
|
|
```bash
|
|
catkin_make --pkg mission_adapters
|
|
source devel/setup.bash
|
|
|
|
PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/mission_adapters/test/config \
|
|
./devel/lib/mission_adapters/plugin_registry_test
|
|
```
|
|
|
|
Nên có test cho: payload hợp lệ, payload thiếu trường, payload chứa NaN/Inf, payload không sinh
|
|
mission nào, và (nếu nguồn có state) hai yêu cầu liên tiếp.
|
|
|
|
## 6. Nguồn không cần `.so`
|
|
|
|
Nguồn biên dịch thẳng vào host thì bỏ qua bước 3 và 4, đăng ký trực tiếp:
|
|
|
|
```cpp
|
|
registry.registerAdapter(std::make_shared<PickAndPlaceAdapter>());
|
|
```
|
|
|
|
Đường này dùng trong test và cho host muốn kiểm soát vòng đời adapter; phần còn lại của luồng
|
|
(validate, convert, định tuyến theo schema) giống hệt.
|