optimal & fix file cmake
This commit is contained in:
142
examples/vda5050_order_demo.cpp
Normal file
142
examples/vda5050_order_demo.cpp
Normal 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;
|
||||
}
|
||||
Reference in New Issue
Block a user