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

optimal

Browse Source
This commit is contained in:
duongtd
2026-07-30 09:24:47 +07:00
parent 4762a3032c
commit d8babff20b
50 changed files with 3280 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files

182
src/config/move_base2_config.cpp
View File

@@ -105,6 +105,54 @@ void describeBinding(std::ostringstream& out, const char* name, const ProfileBin
<< " m yaw=" << binding.default_yaw_tolerance << " rad\n";
}
/// Dịch patience gen-1 sang gen-2. Gen-1: mốc + patience luôn ở quá khứ khi patience <= 0, tức là
/// "fail -> recovery NGAY". Gen-2: <= 0 nghĩa là TẮT đồng hồ — ngược nghĩa hoàn toàn. Giữ hành vi
/// cũ bằng cách dịch thành đúng một chu kỳ điều khiển (gen-1 cũng chỉ phản ứng theo cycle).
double legacyPatience(double value, double control_period_s, const char* key)
{
if (value > 0.0)
{
return value;
}
robot::log_warning(
"[move_base2] legacy %s = %.3f: gen-1 hiểu là 'fail -> recovery ngay', gen-2 hiểu là 'tắt "
"đồng hồ'. Dịch thành một chu kỳ điều khiển (%.4f s) để giữ hành vi cũ.",
key, value, control_period_s);
return control_period_s;
}
/// Đọc binding của một profile theo schema gen-1: tên local planner ở khoá `<profile>_planner_name`
/// tại root, global planner ở section con mang TÊN planner đó (thiếu thì dùng global mặc định).
void readLegacyBinding(robot::NodeHandle& nh, const std::string& name_key,
const std::string& default_global, double xy_tolerance,
double yaw_tolerance, ProfileBinding& binding)
{
binding.default_xy_tolerance = xy_tolerance;
binding.default_yaw_tolerance = yaw_tolerance;
binding.global_planner_name = default_global;
// Default để RỖNG chứ không lấy default gen-1 ("mkt_algorithm/..."): các plugin đó không tồn tại
// trong workspace, và profile không khai coi như không dùng — validate sẽ chặn nếu cả bốn rỗng.
std::string local_name;
nh.param(name_key, local_name, std::string(""));
if (local_name.empty())
{
robot::log_warning("[move_base2] legacy: thiếu '%s' — profile này bị tắt", name_key.c_str());
return;
}
binding.local_planner_name = local_name;
robot::NodeHandle planner_nh(nh, local_name);
if (planner_nh.hasParam("base_global_planner"))
{
planner_nh.param("base_global_planner", binding.global_planner_name,
binding.global_planner_name);
}
robot::log_info("[move_base2] legacy: %s='%s' -> local='%s' global='%s'", name_key.c_str(),
local_name.c_str(), binding.local_planner_name.c_str(),
binding.global_planner_name.c_str());
}
} // namespace
void MoveBase2Config::fromNodeHandle(robot::NodeHandle& nh)
@@ -243,6 +291,140 @@ std::string MoveBase2Config::describe() const
return out.str();
}
// ================================================================================================
// Đọc theo schema gen-1
// ================================================================================================
void MoveBase2Config::fromLegacyNodeHandle(robot::NodeHandle& nh)
{
// Default của gen-1 khác gen-2 ở hai chỗ; chế độ legacy giữ default gen-1 để không đổi hành vi
// của một hệ đang chạy chỉ vì đổi runtime.
robot_base_frame = "base_footprint";
position.default_xy_tolerance = 0.2; // [m]
position.default_yaw_tolerance = 0.2; // [rad]
readDouble(nh, "controller_frequency", controller_frequency);
readDouble(nh, "planner_frequency", planner_frequency);
readDouble(nh, "planner_patience", state_machine.planner_patience);
readDouble(nh, "controller_patience", state_machine.controller_patience);
readDouble(nh, "oscillation_timeout", state_machine.oscillation_timeout);
readDouble(nh, "oscillation_distance", state_machine.oscillation_distance);
readInt(nh, "max_planning_retries", state_machine.max_planning_retries);
readBool(nh, "recovery_behavior_enabled", state_machine.recovery_enabled);
readString(nh, "global_frame", global_frame);
readString(nh, "robot_base_frame", robot_base_frame);
// Sai số ở root là default chung cho cả bốn profile.
double xy = position.default_xy_tolerance;
double yaw = position.default_yaw_tolerance;
readDouble(nh, "xy_goal_tolerance", xy);
readDouble(nh, "yaw_goal_tolerance", yaw);
std::string root_global_planner;
readString(nh, "base_global_planner", root_global_planner);
if (nh.hasParam("base_local_planner"))
{
std::string adapter;
nh.param("base_local_planner", adapter, adapter);
// `LocalPlannerAdapter` là cầu nhúng planner gen-2 vào move_base gen-1. move_base2 gọi thẳng
// interface gen-2 qua ControllerPort nên không cần cầu đó — bỏ qua CÓ LOG, để không ai tưởng
// khoá này vẫn đang có hiệu lực.
robot::log_warning("[move_base2] schema gen-1: bỏ qua base_local_planner='%s' — move_base2 gọi "
"thẳng local planner, không qua adapter.", adapter.c_str());
}
struct LegacyProfile
{
const char* key;
ProfileBinding* binding;
};
const LegacyProfile profiles[] = {
{ "position_planner_name", &position },
{ "docking_planner_name", &docking },
{ "go_straight_planner_name", &go_straight },
{ "rotate_planner_name", &rotate },
};
for (const LegacyProfile& profile : profiles)
{
profile.binding->default_xy_tolerance = xy;
profile.binding->default_yaw_tolerance = yaw;
if (!nh.hasParam(profile.key))
{
robot::log_warning("[move_base2] schema gen-1: thiếu '%s', profile này sẽ từ chối mọi yêu cầu",
profile.key);
continue;
}
std::string local_planner;
nh.param(profile.key, local_planner, local_planner);
profile.binding->local_planner_name = local_planner;
// Global planner riêng của profile nằm trong namespace mang tên chính planner đó; thiếu thì
// rơi về khoá ở root. Đúng cách bản cũ tra (`NodeHandle(private_nh_, position_planner_name_)`).
profile.binding->global_planner_name = root_global_planner;
if (!local_planner.empty())
{
robot::NodeHandle planner_nh(nh, local_planner);
if (planner_nh.hasParam("base_global_planner"))
{
std::string profile_global;
planner_nh.param("base_global_planner", profile_global, profile_global);
profile.binding->global_planner_name = profile_global;
}
}
}
// Dịch tường minh khác biệt NGỮ NGHĨA của patience. Gen-1: 0 nghĩa là "fail -> recovery NGAY"
// (mốc + 0 luôn ở quá khứ). Gen-2: 0 nghĩa là "tắt đồng hồ" — ngược hẳn. Dịch thành đúng MỘT chu
// kỳ điều khiển: gen-1 cũng chỉ phản ứng được ở độ phân giải cycle nên hành vi giữ nguyên.
const double one_cycle = controller_frequency > 0.0 ? 1.0 / controller_frequency : 0.05; // [s]
if (state_machine.planner_patience <= 0.0)
{
robot::log_warning("[move_base2] schema gen-1: planner_patience <= 0 được dịch thành %.3f s "
"(một chu kỳ điều khiển), không phải 'tắt'.", one_cycle);
state_machine.planner_patience = one_cycle;
}
if (state_machine.controller_patience <= 0.0)
{
robot::log_warning("[move_base2] schema gen-1: controller_patience <= 0 được dịch thành %.3f s "
"(một chu kỳ điều khiển), không phải 'tắt'.", one_cycle);
state_machine.controller_patience = one_cycle;
}
}
MoveBase2Config MoveBase2Config::load(robot::NodeHandle& root_nh)
{
MoveBase2Config config;
// Nhận diện schema bằng một khoá bắt buộc phải có ở cả hai. KHÔNG trộn từng khoá giữa hai schema:
// hai nguồn cùng có hiệu lực cho một tham số là đúng kiểu lỗi "sửa config mãi không ăn" mà hai cây
// config trùng tên của workspace này đã gây ra một lần.
robot::NodeHandle modern_nh(root_nh, "move_base2");
if (modern_nh.hasParam("controller_frequency"))
{
robot::log_info("[move_base2] dùng schema mới (namespace 'move_base2').");
config.fromNodeHandle(modern_nh);
return config;
}
if (root_nh.hasParam("controller_frequency") || root_nh.hasParam("base_global_planner"))
{
robot::log_warning("[move_base2] không thấy namespace 'move_base2'; đọc theo schema gen-1 của "
"move_base_common_params.yaml.");
config.fromLegacyNodeHandle(root_nh);
return config;
}
robot::log_error("[move_base2] không tìm thấy cấu hình nào — chạy với toàn bộ giá trị mặc định. "
"Kiểm PNKX_NAV_CORE_CONFIG_DIR và sự tồn tại của file config.");
return config;
}
ControlLoopConfig MoveBase2Config::toControlLoopConfig() const
{
ControlLoopConfig config;