optimal & fix file cmake

This commit is contained in:
2026-08-03 22:40:26 +07:00
parent 33ee9b7f31
commit 2223453639
120 changed files with 12204 additions and 1886 deletions

View File

@@ -0,0 +1,577 @@
#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)