578 lines
22 KiB
C++
578 lines
22 KiB
C++
#include "vda5050_source_adapter.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <string>
|
|
#include <utility>
|
|
|
|
#include <yaml-cpp/yaml.h>
|
|
|
|
#include <boost/dll/alias.hpp>
|
|
|
|
#include <robot/robot.h>
|
|
|
|
namespace mission_plugins
|
|
{
|
|
namespace
|
|
{
|
|
bool isNavigationProfile(const std::string& profile)
|
|
{
|
|
return profile.empty() || profile == "position" || profile == "docking" ||
|
|
profile == "go_straight" || profile == "rotate";
|
|
}
|
|
|
|
using mission_adapters::Action;
|
|
using mission_adapters::ActionType;
|
|
using mission_adapters::ConversionResult;
|
|
using mission_adapters::Mission;
|
|
using mission_adapters::MissionType;
|
|
using mission_adapters::SubmitMode;
|
|
|
|
/// Gom action của edge và của node cuối vào mission, sắp theo sequenceId của VDA5050.
|
|
void collectActions(const std::shared_ptr<Mission>& mission)
|
|
{
|
|
for (const auto& edge : mission->edges)
|
|
{
|
|
for (const auto& action : edge.actions)
|
|
{
|
|
Action ma;
|
|
ma.sequenceId = edge.sequenceId;
|
|
ma.type = ActionType::EDGE_ACTION;
|
|
ma.action = action;
|
|
mission->actions.push_back(std::move(ma));
|
|
}
|
|
}
|
|
|
|
if (!mission->nodes.empty())
|
|
{
|
|
const auto& node = mission->nodes.back();
|
|
for (const auto& action : node.actions)
|
|
{
|
|
Action ma;
|
|
ma.sequenceId = node.sequenceId;
|
|
ma.type = ActionType::NODE_ACTION;
|
|
ma.action = action;
|
|
mission->actions.push_back(std::move(ma));
|
|
}
|
|
}
|
|
|
|
// stable_sort chứ không sort: `sequenceId` ở đây là của NODE/EDGE sở hữu action, nên mọi
|
|
// action trên cùng một node có khoá BẰNG NHAU. Với khoá bằng nhau, std::sort không bảo đảm
|
|
// giữ thứ tự — mà thứ tự đó chính là thứ tự trong mảng JSON, thứ VDA5050 quy định là thứ tự
|
|
// thực hiện. Bản cũ chạy đúng chỉ nhờ libstdc++ dùng insertion sort cho dải nhỏ.
|
|
std::stable_sort(mission->actions.begin(), mission->actions.end(),
|
|
[](const Action& a, const Action& b) {
|
|
return a.sequenceId < b.sequenceId;
|
|
});
|
|
}
|
|
|
|
} // namespace
|
|
|
|
mission_adapters::MissionSourceAdapter::Ptr VDA5050SourceAdapter::create()
|
|
{
|
|
return std::make_shared<VDA5050SourceAdapter>();
|
|
}
|
|
|
|
bool VDA5050SourceAdapter::configure(const std::string& name, robot::NodeHandle& nh)
|
|
{
|
|
name_ = name;
|
|
|
|
nh.getParam(name + "/global_frame", global_frame_, std::string("map"));
|
|
if (global_frame_.empty())
|
|
{
|
|
robot::log_error("VDA5050SourceAdapter[%s]: global_frame is empty", name.c_str());
|
|
return false;
|
|
}
|
|
|
|
if (!loadCompoundActions(name, nh))
|
|
return false;
|
|
|
|
last_order_id_.clear();
|
|
last_order_update_id_ = 0;
|
|
converted_node_count_ = 0;
|
|
return true;
|
|
}
|
|
|
|
bool VDA5050SourceAdapter::loadCompoundActions(const std::string& name, robot::NodeHandle& nh)
|
|
{
|
|
compound_actions_.clear();
|
|
|
|
YAML::Node table;
|
|
if (!nh.getParam(name + "/compound_actions", table) || !table.IsMap())
|
|
return true; // Không khai bảng là hợp lệ: adapter chạy y như trước.
|
|
|
|
for (auto entry = table.begin(); entry != table.end(); ++entry)
|
|
{
|
|
const std::string action_type = entry->first.as<std::string>();
|
|
const YAML::Node& steps_node = entry->second["steps"];
|
|
|
|
if (action_type.empty() || !steps_node || !steps_node.IsSequence() || steps_node.size() == 0)
|
|
{
|
|
robot::log_error("VDA5050SourceAdapter[%s]: compound action '%s' has no 'steps' list",
|
|
name.c_str(), action_type.c_str());
|
|
return false;
|
|
}
|
|
|
|
Steps steps;
|
|
for (std::size_t i = 0; i < steps_node.size(); ++i)
|
|
{
|
|
const YAML::Node& n = steps_node[i];
|
|
Step step;
|
|
|
|
try
|
|
{
|
|
if (n["action"]) step.action = n["action"].as<std::string>();
|
|
if (n["move_to"]) step.move_to = n["move_to"].as<std::string>();
|
|
if (n["move_to_param"]) step.move_to_param = n["move_to_param"].as<std::string>();
|
|
if (n["move"]) step.move = n["move"].as<double>();
|
|
if (n["profile"]) step.motion_hint = n["profile"].as<std::string>();
|
|
if (n["marker"]) step.marker = n["marker"].as<std::string>();
|
|
}
|
|
catch (const YAML::Exception& ex)
|
|
{
|
|
robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu is malformed: %s",
|
|
name.c_str(), action_type.c_str(), i, ex.what());
|
|
return false;
|
|
}
|
|
|
|
const int keys = (step.action.empty() ? 0 : 1) + (step.move_to.empty() ? 0 : 1) +
|
|
(step.move_to_param.empty() ? 0 : 1) +
|
|
(std::isfinite(step.move) ? 1 : 0);
|
|
if (keys != 1)
|
|
{
|
|
// Không đúng một từ khoá thì không có cách diễn giải nào là hiển nhiên đúng. Chặn ở
|
|
// boot thay vì đoán lúc order đầu tiên tới.
|
|
robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu must have exactly one of "
|
|
"action / move_to / move_to_param / move (found %d)",
|
|
name.c_str(), action_type.c_str(), i, keys);
|
|
return false;
|
|
}
|
|
|
|
if (!isNavigationProfile(step.motion_hint))
|
|
{
|
|
robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu has invalid profile '%s'",
|
|
name.c_str(), action_type.c_str(), i, step.motion_hint.c_str());
|
|
return false;
|
|
}
|
|
if (!step.action.empty() && !step.motion_hint.empty())
|
|
{
|
|
robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu is action-only and must "
|
|
"not set profile",
|
|
name.c_str(), action_type.c_str(), i);
|
|
return false;
|
|
}
|
|
if (!step.marker.empty() && step.motion_hint != "docking")
|
|
{
|
|
robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu sets marker but is not "
|
|
"a docking navigation step",
|
|
name.c_str(), action_type.c_str(), i);
|
|
return false;
|
|
}
|
|
|
|
steps.push_back(step);
|
|
}
|
|
|
|
compound_actions_[action_type] = std::move(steps);
|
|
}
|
|
|
|
// Chống đệ quy: một step sinh ra actionType mà chính nó cũng là compound thì expander sẽ mở rộng
|
|
// output của mình — vòng lặp vô hạn lúc convert.
|
|
for (const auto& entry : compound_actions_)
|
|
{
|
|
for (const Step& step : entry.second)
|
|
{
|
|
if (!step.action.empty() && compound_actions_.count(step.action) != 0)
|
|
{
|
|
robot::log_error("VDA5050SourceAdapter[%s]: compound '%s' emits '%s' which is itself "
|
|
"a compound action — that would recurse",
|
|
name.c_str(), entry.first.c_str(), step.action.c_str());
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
robot::log_info("VDA5050SourceAdapter[%s]: %zu compound action(s) loaded", name.c_str(),
|
|
compound_actions_.size());
|
|
return true;
|
|
}
|
|
|
|
const VDA5050SourceAdapter::Steps*
|
|
VDA5050SourceAdapter::findCompound(const std::string& action_type) const
|
|
{
|
|
const auto it = compound_actions_.find(action_type);
|
|
return it == compound_actions_.end() ? nullptr : &it->second;
|
|
}
|
|
|
|
std::string VDA5050SourceAdapter::schema() const
|
|
{
|
|
return mission_adapters::schema::kVda5050Order;
|
|
}
|
|
|
|
size_t VDA5050SourceAdapter::countReleasedNodes(const robot_protocol_msgs::Order& order)
|
|
{
|
|
// VDA5050: base là tiền tố của danh sách node — dừng ở node đầu tiên chưa release.
|
|
size_t count = 0;
|
|
while (count < order.nodes.size() && order.nodes[count].released)
|
|
++count;
|
|
|
|
return count;
|
|
}
|
|
|
|
size_t VDA5050SourceAdapter::countReleasedEdges(const robot_protocol_msgs::Order& order,
|
|
size_t released_node_count)
|
|
{
|
|
if (released_node_count < 2)
|
|
return 0;
|
|
|
|
size_t count = 0;
|
|
const size_t limit = std::min(order.edges.size(), released_node_count - 1);
|
|
while (count < limit && order.edges[count].released)
|
|
++count;
|
|
|
|
return count;
|
|
}
|
|
|
|
size_t VDA5050SourceAdapter::executableNodeCount(const robot_protocol_msgs::Order& order) const
|
|
{
|
|
const size_t released = countReleasedNodes(order);
|
|
if (released > 0)
|
|
return released;
|
|
|
|
// Không node nào released. Theo VDA5050 order phải có ít nhất một base node, nên trường hợp này
|
|
// gần như luôn là host không điền `released`. Coi cả order là base — im lặng không chạy gì sẽ
|
|
// khiến fleet manager chờ vô hạn mà không có dấu hiệu nào.
|
|
robot::log_warning("VDA5050SourceAdapter[%s]: order '%s' has no released node — treating the "
|
|
"whole order as base (did the host leave the 'released' field out?)",
|
|
name_.c_str(), order.orderId.c_str());
|
|
return order.nodes.size();
|
|
}
|
|
|
|
robot_geometry_msgs::PoseStamped
|
|
VDA5050SourceAdapter::toPose(const robot_protocol_msgs::Node& node) const
|
|
{
|
|
robot_geometry_msgs::PoseStamped pose;
|
|
pose.header.frame_id = global_frame_;
|
|
pose.pose.position.x = node.nodePosition.x; // [m]
|
|
pose.pose.position.y = node.nodePosition.y; // [m]
|
|
pose.pose.position.z = 0.0;
|
|
|
|
// theta [rad] quanh trục z -> quaternion.
|
|
const double half_theta = 0.5 * node.nodePosition.theta;
|
|
pose.pose.orientation.x = 0.0;
|
|
pose.pose.orientation.y = 0.0;
|
|
pose.pose.orientation.z = std::sin(half_theta);
|
|
pose.pose.orientation.w = std::cos(half_theta);
|
|
|
|
return pose;
|
|
}
|
|
|
|
bool VDA5050SourceAdapter::validate(const mission_adapters::MissionRequest& request,
|
|
std::string& reason) const
|
|
{
|
|
if (!request.order)
|
|
{
|
|
reason = "request is missing the order payload";
|
|
return false;
|
|
}
|
|
|
|
const auto& order = *request.order;
|
|
|
|
if (order.nodes.empty())
|
|
{
|
|
reason = "order has no node";
|
|
return false;
|
|
}
|
|
|
|
// VDA5050: n node liên thông cần đúng n-1 edge. Thiếu edge nghĩa là đồ thị đứt đoạn, cắt chặng
|
|
// theo chỉ số sẽ lấy nhầm edge của đoạn khác.
|
|
if (order.nodes.size() > 1 && order.edges.size() < order.nodes.size() - 1)
|
|
{
|
|
reason = "fewer edges than nodes - 1";
|
|
return false;
|
|
}
|
|
|
|
for (const auto& node : order.nodes)
|
|
{
|
|
if (!std::isfinite(node.nodePosition.x) || !std::isfinite(node.nodePosition.y) ||
|
|
!std::isfinite(node.nodePosition.theta))
|
|
{
|
|
reason = "nodePosition contains NaN/Inf at node '" + node.nodeId + "'";
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Bản cập nhật cũ hơn (hoặc phát lại) của order đang chạy: từ chối thay vì chạy lại tuyến đường.
|
|
if (!order.orderId.empty() && order.orderId == last_order_id_ &&
|
|
order.orderUpdateId <= last_order_update_id_)
|
|
{
|
|
reason = "orderUpdateId " + std::to_string(order.orderUpdateId) +
|
|
" is not newer than the running one (" + std::to_string(last_order_update_id_) + ")";
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool VDA5050SourceAdapter::expandCompound(
|
|
const std::shared_ptr<mission_adapters::Mission>& leg,
|
|
std::vector<std::shared_ptr<mission_adapters::Mission>>& out) const
|
|
{
|
|
if (compound_actions_.empty() || leg->actions.empty())
|
|
return true; // Không có gì để mở rộng — đường đi thường.
|
|
|
|
std::vector<Action> buffer; // action thường đang chờ xả
|
|
std::vector<Action> on_the_leg; // bộ đệm ĐẦU TIÊN: ở lại trên chặng nav
|
|
bool first_flush = true;
|
|
bool expanded_any = false;
|
|
|
|
// Chặng chỉ-action sinh ra từ bộ đệm sau lần xả đầu.
|
|
auto flush = [&](void) {
|
|
if (buffer.empty())
|
|
return;
|
|
if (first_flush)
|
|
{
|
|
on_the_leg = buffer;
|
|
}
|
|
else
|
|
{
|
|
auto extra = std::make_shared<Mission>();
|
|
extra->type = MissionType::VDA5050_ORDER;
|
|
extra->has_goal = false;
|
|
extra->actions = buffer;
|
|
out.push_back(std::move(extra));
|
|
}
|
|
buffer.clear();
|
|
};
|
|
|
|
for (const Action& entry : leg->actions)
|
|
{
|
|
// Compound chỉ áp cho NODE_ACTION: một chuỗi dò-rồi-tiến-vào không có nghĩa khi gắn vào một
|
|
// cạnh mà robot đang đi trên đó.
|
|
const Steps* steps = (entry.type == ActionType::NODE_ACTION)
|
|
? findCompound(entry.action.actionType)
|
|
: nullptr;
|
|
|
|
if (steps == nullptr)
|
|
{
|
|
if (entry.type == ActionType::EDGE_ACTION &&
|
|
findCompound(entry.action.actionType) != nullptr)
|
|
{
|
|
robot::log_warning("VDA5050SourceAdapter[%s]: '%s' is a compound action but sits on "
|
|
"an EDGE — running it as a plain action",
|
|
name_.c_str(), entry.action.actionType.c_str());
|
|
}
|
|
buffer.push_back(entry);
|
|
continue;
|
|
}
|
|
|
|
flush();
|
|
first_flush = false;
|
|
expanded_any = true;
|
|
|
|
for (const Step& step : *steps)
|
|
{
|
|
auto sub = std::make_shared<Mission>();
|
|
sub->type = MissionType::VDA5050_ORDER;
|
|
|
|
if (!step.action.empty())
|
|
{
|
|
// Action nội bộ: mang NGUYÊN actionParameters của action gốc — handler cần biết dò
|
|
// trạm nào, và nó là chỗ duy nhất hiểu ý nghĩa các tham số đó.
|
|
Action generated;
|
|
generated.type = ActionType::NODE_ACTION;
|
|
generated.sequenceId = entry.sequenceId;
|
|
generated.action = entry.action;
|
|
generated.action.actionType = step.action;
|
|
generated.action.actionId = entry.action.actionId + "-" + step.action;
|
|
|
|
sub->has_goal = false;
|
|
sub->actions.push_back(std::move(generated));
|
|
}
|
|
else
|
|
{
|
|
sub->has_goal = true;
|
|
// Mission nav luôn tự mô tả profile hiệu lực. Action-only giữ chuỗi rỗng để không
|
|
// bao giờ bị hiểu nhầm là một yêu cầu điều khiển.
|
|
sub->motion_hint = step.motion_hint.empty() ? "position" : step.motion_hint;
|
|
sub->marker = step.marker;
|
|
sub->start = leg->start;
|
|
sub->goal = leg->goal; // chỗ dựa; đích thật đến muộn qua goal_frame/relative_distance
|
|
|
|
if (!step.move_to.empty())
|
|
{
|
|
sub->goal_frame = step.move_to;
|
|
}
|
|
else if (!step.move_to_param.empty())
|
|
{
|
|
// Thay thế xảy ra tại CONVERT: runtime không bao giờ thấy placeholder, và thiếu
|
|
// tham số thì order bị từ chối trước khi robot nhúc nhích.
|
|
const auto it = std::find_if(
|
|
entry.action.actionParameters.begin(), entry.action.actionParameters.end(),
|
|
[&step](const robot_protocol_msgs::ActionParameter& p) {
|
|
return p.key == step.move_to_param;
|
|
});
|
|
|
|
if (it == entry.action.actionParameters.end() || it->value.empty())
|
|
{
|
|
robot::log_error("VDA5050SourceAdapter[%s]: compound action '%s' (actionId "
|
|
"'%s') requires parameter '%s' but it is missing",
|
|
name_.c_str(), entry.action.actionType.c_str(),
|
|
entry.action.actionId.c_str(), step.move_to_param.c_str());
|
|
return false;
|
|
}
|
|
sub->goal_frame = it->value;
|
|
}
|
|
else
|
|
{
|
|
sub->relative_distance = step.move;
|
|
}
|
|
}
|
|
|
|
out.push_back(std::move(sub));
|
|
}
|
|
}
|
|
|
|
flush();
|
|
|
|
if (expanded_any)
|
|
leg->actions = on_the_leg;
|
|
|
|
return true;
|
|
}
|
|
|
|
ConversionResult VDA5050SourceAdapter::convert(const mission_adapters::MissionRequest& request)
|
|
{
|
|
ConversionResult result;
|
|
|
|
std::string reason;
|
|
if (!validate(request, reason))
|
|
{
|
|
robot::log_warning("VDA5050SourceAdapter[%s]: dropping the order — %s", name_.c_str(),
|
|
reason.c_str());
|
|
return result;
|
|
}
|
|
|
|
const auto& order = *request.order;
|
|
|
|
const size_t node_count = executableNodeCount(order);
|
|
const size_t edge_count = (countReleasedNodes(order) > 0)
|
|
? countReleasedEdges(order, node_count)
|
|
: order.edges.size();
|
|
|
|
// Order update của đúng order đang chạy: chỉ sinh phần vừa được release thêm.
|
|
const bool is_update = !order.orderId.empty() && order.orderId == last_order_id_;
|
|
|
|
size_t start_node_idx = 0;
|
|
if (is_update)
|
|
{
|
|
if (converted_node_count_ >= node_count)
|
|
{
|
|
robot::log_info("VDA5050SourceAdapter[%s]: order '%s' update %u released no further "
|
|
"node — there is no new work",
|
|
name_.c_str(), order.orderId.c_str(),
|
|
static_cast<unsigned>(order.orderUpdateId));
|
|
|
|
last_order_update_id_ = order.orderUpdateId;
|
|
return result;
|
|
}
|
|
|
|
// Node cuối đã chuyển đổi trở thành node xuất phát của chặng tiếp theo.
|
|
start_node_idx = converted_node_count_ - 1;
|
|
result.mode = SubmitMode::kAppend;
|
|
}
|
|
|
|
// Chặng được cắt tại mỗi node có action: robot chạy tới node đó rồi mới thực hiện action.
|
|
std::vector<size_t> action_node_indices;
|
|
for (size_t i = start_node_idx + 1; i < node_count; ++i)
|
|
{
|
|
if (!order.nodes[i].actions.empty())
|
|
action_node_indices.push_back(i);
|
|
}
|
|
|
|
// Node xuất phát có action (và chưa từng được chuyển đổi) cũng là một chặng — chặng chỉ-action.
|
|
if (!is_update && !order.nodes[start_node_idx].actions.empty())
|
|
action_node_indices.insert(action_node_indices.begin(), start_node_idx);
|
|
|
|
auto makeMission = [&](size_t from, size_t to) {
|
|
auto mission = std::make_shared<Mission>();
|
|
mission->type = MissionType::VDA5050_ORDER;
|
|
|
|
mission->nodes.assign(order.nodes.begin() + from, order.nodes.begin() + to + 1);
|
|
|
|
if (to > from)
|
|
{
|
|
const size_t edge_to = std::min(to, edge_count);
|
|
if (from < edge_to)
|
|
mission->edges.assign(order.edges.begin() + from, order.edges.begin() + edge_to);
|
|
}
|
|
|
|
// D8: chặng một node là chặng chỉ-có-action (action nằm ngay tại node xuất phát) — không có
|
|
// quãng đường nào để đi. Quy tắc thuần cấu trúc, adapter không cần biết robot đang ở đâu.
|
|
mission->has_goal = (to > from);
|
|
|
|
if (mission->has_goal)
|
|
{
|
|
// Mission self-contained: consumer không phải suy goal ra từ nodes (A4).
|
|
mission->start = toPose(order.nodes[from]);
|
|
mission->goal = toPose(order.nodes[to]);
|
|
mission->motion_hint = "position";
|
|
}
|
|
|
|
collectActions(mission);
|
|
return mission;
|
|
};
|
|
|
|
// Mission không goal mà cũng không action là mission rỗng — không sinh ra nó.
|
|
auto isMeaningful = [](const std::shared_ptr<Mission>& mission) {
|
|
return mission->has_goal || !mission->actions.empty();
|
|
};
|
|
|
|
size_t segment_start = start_node_idx;
|
|
|
|
for (const size_t action_node_idx : action_node_indices)
|
|
{
|
|
auto mission = makeMission(segment_start, action_node_idx);
|
|
|
|
// Mở rộng TRƯỚC isMeaningful: hàm này gỡ action compound khỏi chặng, và một chặng chỉ-action
|
|
// sau khi gỡ có thể trở thành rỗng.
|
|
std::vector<std::shared_ptr<Mission>> expanded;
|
|
if (!expandCompound(mission, expanded))
|
|
{
|
|
// Thiếu tham số cấu trúc: bỏ CẢ order. Một order chạy nửa vời — robot tới trạm sạc rồi
|
|
// không sạc — nguy hiểm hơn là không chạy.
|
|
result.missions.clear();
|
|
return result;
|
|
}
|
|
|
|
if (isMeaningful(mission))
|
|
result.missions.push_back(std::move(mission));
|
|
|
|
for (auto& sub : expanded)
|
|
result.missions.push_back(std::move(sub));
|
|
|
|
segment_start = action_node_idx;
|
|
}
|
|
|
|
// Đoạn còn lại sau node có action cuối cùng.
|
|
if (segment_start + 1 < node_count)
|
|
{
|
|
auto mission = makeMission(segment_start, node_count - 1);
|
|
if (isMeaningful(mission))
|
|
result.missions.push_back(std::move(mission));
|
|
}
|
|
|
|
if (result.missions.empty())
|
|
return result;
|
|
|
|
last_order_id_ = order.orderId;
|
|
last_order_update_id_ = order.orderUpdateId;
|
|
converted_node_count_ = node_count;
|
|
|
|
return result;
|
|
}
|
|
|
|
} // namespace mission_plugins
|
|
|
|
BOOST_DLL_ALIAS(mission_plugins::VDA5050SourceAdapter::create, VDA5050SourceAdapter)
|