143 lines
5.4 KiB
C++
143 lines
5.4 KiB
C++
/**
|
|
* @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;
|
|
}
|