DuongTD/mission_adapters
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

optimal & fix file cmake

Browse Source
This commit is contained in:
duongtd
2026-08-03 22:40:26 +07:00
parent 33ee9b7f31
commit 2223453639
120 changed files with 12204 additions and 1886 deletions
Download Patch File Download Diff File Expand all files Collapse all files

101
examples/robot_control_example.cpp Normal file
View File

@@ -0,0 +1,101 @@
/*********************************************************************
*
* 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;
}

142
examples/vda5050_order_demo.cpp Normal file
View File

@@ -0,0 +1,142 @@
/**
* @file vda5050_order_demo.cpp
* @brief Chạy một VDA5050 Order qua VDA5050SourceAdapter và in ra các chặng thu được.
*
* Order dùng ở đây lấy nguyên toạ độ từ một order thật do fleet manager gửi xuống (4 node, 3 edge,
* edge cuối có NURBS bậc 2), chỉ khác một điểm: gắn thêm một action vào **node thứ 2**. Mục đích là
* thấy tận mắt adapter cắt order thành chặng như thế nào khi có action ở giữa đường.
*
* Chạy: ./devel/lib/mission_adapters/vda5050_order_demo
*/
#include "mission_adapters/mission_request.h"
#include "mission_adapters/types.h"
#include "../plugins/vda5050_source_adapter.h"
#include <cstdio>
#include <string>
namespace
{
/// Một node released, kèm toạ độ thật lấy từ order của fleet.
robot_protocol_msgs::Node makeNode(int sequence_id, const std::string& id, double x, double y)
{
robot_protocol_msgs::Node node;
node.sequenceId = sequence_id;
node.nodeId = id;
node.released = true;
node.nodePosition.x = x;
node.nodePosition.y = y;
node.nodePosition.mapId = "97b3bf93-10c5-4c79-2cc9-08deee20817a";
return node;
}
robot_protocol_msgs::Edge makeEdge(int sequence_id, const std::string& id,
const std::string& start_id, const std::string& end_id)
{
robot_protocol_msgs::Edge edge;
edge.sequenceId = sequence_id;
edge.edgeId = id;
edge.released = true;
edge.startNodeId = start_id;
edge.endNodeId = end_id;
return edge;
}
robot_protocol_msgs::Action makeAction(const std::string& id, const std::string& type)
{
robot_protocol_msgs::Action action;
action.actionId = id;
action.actionType = type;
action.blockingType = "HARD";
return action;
}
void printOrder(const robot_protocol_msgs::Order& order)
{
std::printf("ORDER GUI XUONG (orderId=%s, orderUpdateId=%d)\n",
order.orderId.c_str(), (int)order.orderUpdateId);
for (const auto& n : order.nodes)
std::printf(" node seq=%-2d %-10s pos=(%7.3f, %7.3f) actions=%zu%s\n",
(int)n.sequenceId, n.nodeId.c_str(),
n.nodePosition.x, n.nodePosition.y, n.actions.size(),
n.actions.empty() ? "" : (" <-- " + n.actions.front().actionType).c_str());
for (const auto& e : order.edges)
std::printf(" edge seq=%-2d %-10s %s -> %s actions=%zu\n",
(int)e.sequenceId, e.edgeId.c_str(),
e.startNodeId.c_str(), e.endNodeId.c_str(), e.actions.size());
}
void printMissions(const std::vector<std::shared_ptr<mission_adapters::Mission>>& missions)
{
std::printf("\nKET QUA: adapter cat thanh %zu chang\n", missions.size());
std::printf("%s\n", std::string(78, '-').c_str());
int idx = 0;
for (const auto& m : missions)
{
std::printf(" Chang #%d\n", ++idx);
std::printf(" type : %s\n",
m->type == mission_adapters::MissionType::VDA5050_ORDER ? "VDA5050_ORDER"
: "SIMPLE_GOAL");
std::printf(" has_goal : %s\n", m->has_goal ? "true" : "false");
if (m->has_goal)
std::printf(" di chuyen : (%7.3f, %7.3f) -> (%7.3f, %7.3f)\n",
m->start.pose.position.x, m->start.pose.position.y,
m->goal.pose.position.x, m->goal.pose.position.y);
else
std::printf(" di chuyen : (khong - chang chi-co-action)\n");
std::printf(" nodes : %zu, edges: %zu\n", m->nodes.size(), m->edges.size());
if (m->actions.empty())
{
std::printf(" actions : (khong co)\n");
}
else
{
for (const auto& a : m->actions)
std::printf(" actions : seq=%d %-12s id=%s (%s)\n",
a.sequenceId, a.action.actionType.c_str(), a.action.actionId.c_str(),
a.type == mission_adapters::ActionType::NODE_ACTION ? "NODE_ACTION"
: "EDGE_ACTION");
}
std::printf("\n");
}
}
} // namespace
int main()
{
// Toa do lay tu order that: 14:39:21, orderId c16cd9e3..., 4 node / 3 edge.
robot_protocol_msgs::Order order;
order.orderId = "demo-dua-tren-order-that";
order.orderUpdateId = 0;
order.nodes.push_back(makeNode(0, "node_A", 10.010, 10.000));
order.nodes.push_back(makeNode(2, "node_B", 10.260, 10.192));
order.nodes.push_back(makeNode(4, "node_C", 14.979, 7.599));
order.nodes.push_back(makeNode(6, "node_D", 12.986, 2.851));
// ĐIỂM KHÁC DUY NHẤT so với order thật: mot action gan vao NODE THU 2.
order.nodes[1].actions.push_back(makeAction("act-tai-node-2", "PickUp"));
order.edges.push_back(makeEdge(1, "edge_AB", "node_A", "node_B"));
order.edges.push_back(makeEdge(3, "edge_BC", "node_B", "node_C"));
order.edges.push_back(makeEdge(5, "edge_CD", "node_C", "node_D"));
printOrder(order);
mission_plugins::VDA5050SourceAdapter adapter;
std::string reason;
const auto request = mission_adapters::MissionRequest::fromOrder(order);
if (!adapter.validate(request, reason))
{
std::printf("\nAdapter TU CHOI order: %s\n", reason.c_str());
return 1;
}
printMissions(adapter.convert(request).missions);
return 0;
}