102 lines
3.6 KiB
C++
102 lines
3.6 KiB
C++
/*********************************************************************
|
|
*
|
|
* Ví dụ tối thiểu: nối mission layer vào một navigation runtime.
|
|
*
|
|
* Điểm chính của ví dụ là hai chiều dữ liệu, không phải thuật toán:
|
|
*
|
|
* host --goalEvent/orderEvent--> EventProcessor --> MissionManager
|
|
* |
|
|
* MissionExecutor
|
|
* |
|
|
* NavigationClient::dispatch/cancelActive
|
|
* |
|
|
* host <--navDoneEvent(id)/navFailedEvent(id)-------- navigation runtime
|
|
*
|
|
* Outcome quay về BẮT BUỘC mang MissionId. Bản trước của ví dụ này poll trường trạng thái của
|
|
* navigation rồi suy ra "chặng nào vừa xong" — cách đó gán nhầm kết quả ngay khi có một order thay
|
|
* thế order đang chạy.
|
|
*
|
|
*********************************************************************/
|
|
#include <cstddef>
|
|
#include <memory>
|
|
|
|
#include <robot/robot.h>
|
|
|
|
#include <mission_adapters/mission_adapters.h>
|
|
#include <mission_adapters/plugin_registry.h>
|
|
|
|
namespace
|
|
{
|
|
|
|
/**
|
|
* @brief NavigationClient chỉ ghi log — chỗ để thay bằng runtime thật.
|
|
*
|
|
* Trong hệ thống thật, dispatch() dựng yêu cầu di chuyển từ mission rồi giao cho navigation runtime,
|
|
* và runtime báo ngược kết quả qua navDoneEvent(mission->id) / navFailedEvent(mission->id).
|
|
*/
|
|
class LoggingNavigationClient : public mission_adapters::NavigationClient
|
|
{
|
|
public:
|
|
bool dispatch(const std::shared_ptr<const mission_adapters::Mission>& mission) override
|
|
{
|
|
if (!mission)
|
|
return false;
|
|
|
|
robot::log_info("dispatch mission %lu — %zu action",
|
|
static_cast<unsigned long>(mission->id),
|
|
mission->actions.size());
|
|
return true;
|
|
}
|
|
|
|
void cancelActive(mission_adapters::MissionId id) override
|
|
{
|
|
robot::log_info("cancel mission %lu", static_cast<unsigned long>(id));
|
|
}
|
|
};
|
|
|
|
} // namespace
|
|
|
|
int main(int argc, char** argv)
|
|
{
|
|
robot::init(argc, argv, "mission_adapters_example");
|
|
|
|
// Nguồn mission được nạp từ YAML: thêm loại nguồn mới không phải sửa file này.
|
|
mission_adapters::PluginRegistry registry;
|
|
robot::NodeHandle nh;
|
|
if (!registry.loadFromConfig(nh))
|
|
{
|
|
robot::log_error("could not load enough mission sources — check "
|
|
"mission_adapters_params.yaml");
|
|
return 1;
|
|
}
|
|
|
|
mission_adapters::MissionManager mission_manager;
|
|
mission_adapters::EventProcessor event_processor(mission_manager, registry);
|
|
mission_adapters::MissionExecutor mission_executor(mission_manager);
|
|
|
|
LoggingNavigationClient navigation_client;
|
|
mission_executor.setNavigationClient(&navigation_client);
|
|
|
|
event_processor.start();
|
|
mission_executor.start();
|
|
|
|
// Nguồn mission thật (MQTT/OPC-UA/REST) gọi các hàm *Event() này từ thread của nó.
|
|
robot_geometry_msgs::PoseStamped goal;
|
|
goal.header.frame_id = "map";
|
|
goal.pose.position.x = 2.0; // [m]
|
|
goal.pose.position.y = 1.0; // [m]
|
|
event_processor.goalEvent(goal);
|
|
|
|
robot::Rate rate(20); // [Hz]
|
|
while (robot::ok())
|
|
{
|
|
// Runtime thật gọi navDoneEvent(id) / navFailedEvent(id) ở đây, với id lấy từ chính mission
|
|
// mà dispatch() đã nhận — không suy đoán từ trạng thái navigation.
|
|
rate.sleep();
|
|
}
|
|
|
|
mission_executor.stop();
|
|
event_processor.stop();
|
|
return 0;
|
|
}
|