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;

68
src/control_loop.cpp
View File

@@ -11,6 +11,8 @@
#include <cmath>
#include <sstream>
#include <robot/console.h>
namespace move_base2
{
namespace
@@ -400,6 +402,13 @@ bool ControlLoop::step()
action_feedback_ = ActionFeedback::kIdle;
// --- 3. Thi hành output ---------------------------------------------------------------------
// Nhãn của yêu cầu SẮP bị thay. Preempt bật cả `report_outcome` lẫn `accept_request` trong cùng
// một cycle, mà phần báo kết quả nằm sau phần nhận yêu cầu — không giữ lại id cũ ở đây thì kết
// quả của chặng cũ sẽ được báo dưới id của chặng MỚI, và mission layer mất dấu cả hai.
const std::uint64_t outgoing_mission_id =
has_active_request_ ? active_request_.mission_sequence_id : 0;
const bool had_active_request = has_active_request_;
if (output.accept_request)
{
active_request_ = pending_request_;
@@ -408,6 +417,21 @@ bool ControlLoop::step()
latest_plan_.clear();
has_outcome_ = false;
// Log một lần tại sườn nhận goal — không nằm trên đường lặp của control loop.
if (active_request_.has_goal)
{
robot::log_info("[move_base2] Nhận goal (mission %llu): x=%.3f y=%.3f frame=%s.\n",
static_cast<unsigned long long>(active_request_.mission_sequence_id),
active_request_.goal.pose.position.x, active_request_.goal.pose.position.y,
active_request_.goal.header.frame_id.c_str());
}
else
{
robot::log_info("[move_base2] Nhận yêu cầu chỉ-action (mission %llu), %zu action.\n",
static_cast<unsigned long long>(active_request_.mission_sequence_id),
active_request_.actions.size());
}
// Nhãn mới: mọi lượt lập plan đang bay thuộc về goal cũ và phải bị vứt khi về.
++plan_tag_;
deps_.planner->cancelPlan();
@@ -546,19 +570,49 @@ bool ControlLoop::step()
has_outcome_ = true;
++outcome_report_count_;
if (deps_.mission != nullptr && has_active_request_ &&
active_request_.mission_sequence_id != 0)
// Log kết cục ra terminal — bản cũ báo "Goal reached!" tại đây, host và người vận hành đều
// dựa vào dòng này để biết chặng kết thúc ra sao. Chỉ chạy đúng một lần mỗi chặng.
switch (output.outcome)
{
deps_.mission->reportOutcome(active_request_.mission_sequence_id, output.outcome);
case NavigationOutcome::kSucceeded:
robot::log_success("[move_base2] Goal reached (mission %llu).\n",
static_cast<unsigned long long>(outgoing_mission_id));
break;
case NavigationOutcome::kPreempted:
robot::log_info("[move_base2] Goal bị thay bởi goal mới (mission %llu: PREEMPTED).\n",
static_cast<unsigned long long>(outgoing_mission_id));
break;
case NavigationOutcome::kCancelled:
robot::log_info("[move_base2] Goal bị huỷ (mission %llu: CANCELLED).\n",
static_cast<unsigned long long>(outgoing_mission_id));
break;
case NavigationOutcome::kFailed:
robot::log_error("[move_base2] Navigation thất bại (mission %llu: ABORTED): %s\n",
static_cast<unsigned long long>(outgoing_mission_id),
last_reason_ != nullptr ? last_reason_ : "");
break;
}
has_active_request_ = false;
if (deps_.mission != nullptr && had_active_request && outgoing_mission_id != 0)
{
deps_.mission->reportOutcome(outgoing_mission_id, output.outcome);
}
// Preempt vừa báo kết quả chặng cũ VỪA nhận chặng mới trong cùng cycle. Chỉ xoá yêu cầu đang
// chạy khi thực sự không có chặng mới nào thay thế.
if (!output.accept_request)
{
has_active_request_ = false;
latest_plan_.clear();
planner_running_ = false;
}
cancel_requested_ = false;
latest_plan_.clear();
planner_running_ = false;
}
return !output.report_outcome;
// false = "yêu cầu hiện tại vừa kết thúc, không còn gì để chạy". Preempt CŨNG báo kết quả (cho
// chặng cũ) nhưng ngay lập tức nhận chặng mới — lúc đó runtime vẫn đang chạy, nên không được trả
// false. Điều kiện đúng là "vào state terminal", không phải "có báo kết quả".
return !isTerminal(output.state);
}
const char* ControlLoop::lastOutcome() const

148
src/io/costmap_exporter.cpp Normal file
View File

@@ -0,0 +1,148 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — cài đặt CostmapExporter.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/io/costmap_exporter.h>
#include <array>
#include <boost/thread/locks.hpp>
#include <robot/robot.h>
#include <robot_costmap_2d/cost_values.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/layered_costmap.h>
namespace move_base2
{
namespace
{
/**
* @brief Bảng quy đổi 0–255 của costmap sang −1..100 của OccupancyGrid.
*
* Dựng một lần, bất biến. Bản cũ dùng `new char[256]` gán vào biến static rồi không bao giờ giải
* phóng; ở đây là một mảng có tuổi thọ tĩnh, không cấp phát động.
*/
const std::array<signed char, 256>& costTranslationTable()
{
static const std::array<signed char, 256> table = [] {
std::array<signed char, 256> t{};
// Dải thường: 10..252 của costmap co về 1..98 của OccupancyGrid.
for (int i = static_cast<int>(robot_costmap_2d::FREE_SPACE) + 1;
i < static_cast<int>(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE); ++i)
{
t[static_cast<std::size_t>(i)] = static_cast<signed char>(1 + (97 * (i - 1)) / 251);
}
// Các giá trị đặc biệt, đặt SAU dải thường để không bị nó ghi đè.
t[static_cast<std::size_t>(robot_costmap_2d::PREFERRED_SPACE)] = 120;
for (int i = static_cast<int>(robot_costmap_2d::PREFERRED_SPACE) + 1;
i <= static_cast<int>(robot_costmap_2d::FREE_SPACE); ++i)
{
t[static_cast<std::size_t>(i)] = 0; // không có vật cản
}
t[static_cast<std::size_t>(robot_costmap_2d::CRITICAL_SPACE)] = 150;
t[static_cast<std::size_t>(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)] = 99;
t[static_cast<std::size_t>(robot_costmap_2d::LETHAL_OBSTACLE)] = 100;
t[static_cast<std::size_t>(robot_costmap_2d::NO_INFORMATION)] = -1; // chưa biết
return t;
}();
return table;
}
} // namespace
CostmapExporter::CostmapExporter() = default;
void CostmapExporter::attach(robot_costmap_2d::Costmap2DROBOT* costmap,
const std::string& global_frame)
{
std::lock_guard<std::mutex> lock(mutex_);
costmap_ = costmap;
if (!global_frame.empty())
{
global_frame_ = global_frame;
}
}
void CostmapExporter::prepareGridLocked()
{
robot_costmap_2d::Costmap2D* costmap = costmap_->getCostmap();
const double resolution = costmap->getResolution(); // [m/cell]
grid_.header.frame_id = global_frame_;
grid_.header.stamp = robot::Time::now();
grid_.info.resolution = resolution;
grid_.info.width = costmap->getSizeInCellsX();
grid_.info.height = costmap->getSizeInCellsY();
// Gốc của OccupancyGrid là GÓC ô (0,0), còn mapToWorld trả TÂM ô — lệch nửa ô. Bỏ qua phép trừ
// này thì cả bản đồ trên rviz dịch đi 2.5 cm ở độ phân giải 0.05 m, đủ để mọi thứ trông "gần
// đúng" mà không ai chỉ ra được sai ở đâu.
double wx = 0.0;
double wy = 0.0;
costmap->mapToWorld(0, 0, wx, wy);
grid_.info.origin.position.x = wx - resolution / 2.0; // [m]
grid_.info.origin.position.y = wy - resolution / 2.0; // [m]
grid_.info.origin.position.z = 0.0;
grid_.info.origin.orientation.w = 1.0;
saved_origin_x_ = costmap->getOriginX();
saved_origin_y_ = costmap->getOriginY();
grid_.data.resize(static_cast<std::size_t>(grid_.info.width) * grid_.info.height);
const unsigned char* data = costmap->getCharMap();
const std::array<signed char, 256>& table = costTranslationTable();
for (std::size_t i = 0; i < grid_.data.size(); ++i)
{
grid_.data[i] = table[data[i]];
}
}
void CostmapExporter::fill(robot_nav_msgs::OccupancyGrid& grid,
robot_map_msgs::OccupancyGridUpdate& /*update*/, bool& is_updated)
{
is_updated = false;
std::lock_guard<std::mutex> lock(mutex_);
if (costmap_ == nullptr)
{
return;
}
robot_costmap_2d::Costmap2D* costmap = costmap_->getCostmap();
if (costmap == nullptr || costmap->getSizeInCellsX() == 0 || costmap->getSizeInCellsY() == 0)
{
// Costmap chưa nhận được static map. Trả về mà không ghi gì: publish một lưới rỗng sẽ xoá trắng
// thứ đang hiển thị trên rviz.
return;
}
// Khoá costmap trong lúc đọc: thread cập nhật của costmap đang ghi vào chính mảng này.
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> costmap_lock(*(costmap->getMutex()));
// LUÔN dựng lại lưới đầy đủ.
//
// Đường gửi phần-thay-đổi chỉ đúng khi có ĐÚNG MỘT consumer, mà host thì gọi getGlobalData() từ
// HAI ros::Timer: costmap (1 Hz) và plan (10 Hz). Timer plan tiêu thụ hầu hết các lần gọi rồi vứt
// phần costmap đi, nên tới lượt timer costmap thì vùng thay đổi đã bị reset — và lưới cache chỉ
// được dựng lại khi HÌNH HỌC đổi. Local costmap là rolling window nên gốc toạ độ dịch liên tục và
// vẫn tươi; global costmap hình học cố định nên đóng băng ở ảnh chụp đầu tiên vĩnh viễn.
//
// Bản cũ có cờ `always_send_full_costmap` cho đúng tình huống này. Ở đây chọn luôn gửi đầy đủ:
// 400x400 ô là 160 KB, ở 1-5 Hz là không đáng kể so với một lớp lỗi im lặng.
prepareGridLocked();
grid = grid_;
is_updated = false;
}
} // namespace move_base2

13
src/move_base2_plugin.cpp
View File

@@ -33,3 +33,16 @@ robot::move_base_core::BaseNavigation::Ptr createMoveBase2()
} // namespace move_base2
BOOST_DLL_ALIAS(move_base2::createMoveBase2, MoveBase2)
/**
* Alias thứ hai, cùng trỏ vào một factory.
*
* `amr_control` import alias **"MoveBase"** — tên đó hard-code trong host
* (`amr_control.cpp:244-246`), không đọc từ config. Không export nó thì đổi
* `MoveBase: library_path: libmove_base2` sẽ hỏng lúc chạy với lỗi không tìm thấy symbol, và
* `boost::dll` không kiểm kiểu qua ranh giới `.so` nên thông báo lỗi cũng không nói được vì sao.
*
* Có cả hai alias nghĩa là chuyển đổi giữa hai runtime chỉ tốn đúng một dòng YAML, không phải sửa
* host. `MoveBase2` giữ lại cho host nào muốn nạp tường minh bản mới.
*/
BOOST_DLL_ALIAS(move_base2::createMoveBase2, MoveBase)

202
src/navigation_runtime.cpp Normal file
View File

@@ -0,0 +1,202 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — cài đặt NavigationRuntime.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/navigation_runtime.h>
#include <exception>
#include <robot/node_handle.h>
#include <robot/robot.h>
#include <tf3/buffer_core.h>
namespace move_base2
{
NavigationRuntime::NavigationRuntime() = default;
NavigationRuntime::~NavigationRuntime()
{
// Dừng thread cập nhật costmap trước khi bất kỳ thành viên nào bị huỷ: các runner giữ con trỏ tới
// costmap, và costmap giữ thread đang chạy qua chuỗi layer.
stop();
}
bool NavigationRuntime::buildCostmaps(const std::shared_ptr<tf3::BufferCore>& tf,
std::string& error)
{
if (built_ || costmapsReady())
{
error = "NavigationRuntime::buildCostmaps() gọi lần thứ hai";
return false;
}
if (!tf)
{
error = "NavigationRuntime cần TF buffer khác null";
return false;
}
tf_ = tf;
// --- 1. Config ---------------------------------------------------------------------------------
robot::NodeHandle root_nh("~");
config_ = MoveBase2Config::load(root_nh);
// --- 2. Costmap --------------------------------------------------------------------------------
//
// Dựng nhưng CHƯA start: thread cập nhật chạy trong lúc planner chưa nạp xong là cửa sổ để mọi
// thứ chạm vào nhau ở trạng thái nửa vời. start() nằm ở hàm riêng, gọi sau khi lắp xong.
try
{
global_costmap_.reset(new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_));
local_costmap_.reset(new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_));
}
catch (const std::exception& ex)
{
error = std::string("không dựng được costmap: ") + ex.what();
global_costmap_.reset();
local_costmap_.reset();
return false;
}
global_costmap_->pause();
local_costmap_->pause();
// Hai nguồn pose, khác frame — xem doc của thành viên. Bản cũ cũng vậy: `makePlan` lấy start từ
// `planner_costmap_robot_` (map), còn `LocalPlannerAdapter` lấy pose từ costmap local (odom).
global_pose_.setCostmap(global_costmap_.get());
local_pose_.setCostmap(local_costmap_.get());
// Gắn ngay: host có thể hỏi dữ liệu hiển thị bất cứ lúc nào sau initialize(), kể cả trước khi
// costmap có nội dung. Exporter tự trả về "chưa có gì" thay vì lưới rỗng.
global_exporter_.attach(global_costmap_.get(), config_.global_frame);
local_exporter_.attach(local_costmap_.get(), local_costmap_->getGlobalFrameID());
return true;
}
bool NavigationRuntime::buildRunners(std::string& error)
{
if (built_)
{
error = "NavigationRuntime::buildRunners() gọi lần thứ hai";
return false;
}
if (!costmapsReady())
{
error = "buildRunners() gọi trước buildCostmaps()";
return false;
}
robot::NodeHandle root_nh("~");
// --- 3. Planner và controller ------------------------------------------------------------------
if (!planner_.configure(root_nh, global_costmap_.get(), config_.position.global_planner_name,
error))
{
global_costmap_.reset();
local_costmap_.reset();
return false;
}
if (!controller_.configure(root_nh, tf_, local_costmap_.get(), &local_pose_,
config_.position.local_planner_name, error))
{
global_costmap_.reset();
local_costmap_.reset();
return false;
}
// --- 4. Recovery -------------------------------------------------------------------------------
RecoveryRunner::Deps recovery_deps;
recovery_deps.clock = &clock_;
recovery_deps.pose = &local_pose_;
recovery_deps.local_costmap = local_costmap_.get();
recovery_deps.global_costmap = global_costmap_.get();
recovery_.setDeps(recovery_deps);
recovery_.setNamespace(config_.recovery_namespace);
if (!recovery_.configure(root_nh))
{
// Không dừng lại: một behavior hỏng không nên xoá sạch các đường phục hồi còn lại.
// `behaviorCount()` bên dưới phản ánh số nạp được THẬT, và `validate()` sẽ chặn nếu con số đó
// bằng 0 trong khi recovery vẫn đang bật.
robot::log_warning("[move_base2] NavigationRuntime: có behavior recovery nạp hỏng; chạy tiếp "
"với %zu behavior còn lại.\n", recovery_.behaviorCount());
}
// Ràng buộc thứ tự khởi tạo — xem doc của lớp. Con số này KHÔNG đến từ YAML.
config_.state_machine.recovery_behavior_count = recovery_.behaviorCount();
// --- 5. Action ---------------------------------------------------------------------------------
action_.setClock(&clock_);
action_.setNamespace(config_.action_namespace);
if (!action_.configure(root_nh))
{
robot::log_warning("[move_base2] NavigationRuntime: có action handler nạp hỏng; chạy tiếp với "
"%zu handler còn lại.\n", action_.handlerCount());
}
// --- 6. Kiểm cấu hình sau cùng -----------------------------------------------------------------
if (!config_.validate(error))
{
global_costmap_.reset();
local_costmap_.reset();
return false;
}
robot::log_info("[move_base2] NavigationRuntime dựng xong:\n%s", config_.describe().c_str());
built_ = true;
return true;
}
void NavigationRuntime::start()
{
if (!built_)
{
return;
}
global_costmap_->start();
local_costmap_->start();
mission_.start();
}
void NavigationRuntime::stop()
{
mission_.stop();
if (local_costmap_)
{
local_costmap_->stop();
}
if (global_costmap_)
{
global_costmap_->stop();
}
}
ControlLoopDeps NavigationRuntime::deps()
{
ControlLoopDeps deps;
if (!built_)
{
return deps;
}
deps.clock = &clock_;
deps.pose = &global_pose_; // planner start + chống quẩn + pose báo về host: frame `map`
deps.planner = &planner_;
deps.controller = &controller_;
deps.recovery = &recovery_;
deps.mission = &mission_;
deps.action = &action_;
return deps;
}
} // namespace move_base2

329
src/navigation_server.cpp
View File

@@ -39,7 +39,64 @@ NavigationServer::NavigationServer()
nav_feedback_->is_ready = false;
}
NavigationServer::~NavigationServer() = default;
NavigationServer::~NavigationServer()
{
// Dừng control thread TRƯỚC khi bất kỳ thành viên nào bị huỷ: nó đang chạm loop_, runtime_ và
// sensors_ ở mỗi cycle. Phần join thread còn lại (planner, map update) nằm ở destructor của
// runtime_ và các thành viên, chạy ngay sau đây theo thứ tự khai báo.
shutdown();
}
void NavigationServer::shutdown()
{
stopControlThread();
if (runtime_ != nullptr)
{
runtime_->stop();
}
}
bool NavigationServer::startControlThread(double frequency)
{
if (!loop_.initialized())
{
robot::log_error("[move_base2] startControlThread() trước khi control loop được cấu hình.\n");
return false;
}
if (control_thread_running_.load())
{
return false;
}
if (!(frequency > 0.0))
{
robot::log_error("[move_base2] controller_frequency phải > 0 [Hz], nhận %.3f.\n", frequency);
return false;
}
control_thread_running_.store(true);
control_thread_ = std::thread([this, frequency]() {
robot::Rate rate(frequency);
while (control_thread_running_.load())
{
// Bỏ qua giá trị trả về: false chỉ nghĩa là yêu cầu hiện tại vừa kết thúc, không phải lý do
// dừng vòng lặp — thread phải sống để nhận goal kế tiếp.
spinOnce();
rate.sleep();
}
});
robot::log_info("[move_base2] control thread chạy ở %.2f Hz.\n", frequency);
return true;
}
void NavigationServer::stopControlThread()
{
control_thread_running_.store(false);
if (control_thread_.joinable())
{
control_thread_.join();
}
}
// ================================================================================================
// Cấu hình lõi
@@ -57,6 +114,7 @@ bool NavigationServer::configureLoop(const ControlLoopConfig& config, const Cont
robot_base_frame_ = config.robot_base_frame;
nav_feedback_->is_ready = true;
nav_feedback_->feed_back_str = "sẵn sàng";
refreshFeedback();
@@ -102,9 +160,18 @@ bool NavigationServer::spinOnce()
// không ở cuối cycle để trần vận tốc có hiệu lực ngay trong chính cycle này — chậm một cycle
// nghĩa là một chu kỳ nữa chạy quá tốc độ mà tầng an toàn vừa yêu cầu hạ.
pushHostInputsToController();
drainLifecycleRequests();
// Mission layer cất chặng mới trong bridge; đây là chỗ duy nhất nó được đẩy vào lõi, trên đúng
// control thread.
if (runtime_ != nullptr)
{
runtime_->mission().pumpPendingRequest();
}
const bool running = loop_.step();
publishCommand();
cachePlans();
refreshFeedback();
return running;
}
@@ -116,14 +183,39 @@ void NavigationServer::publishCommand()
// — robot giữ nguyên tốc độ hiện tại vô hạn và VelocityArbiter bị vô hiệu hoàn toàn.
//
// Nguồn duy nhất đúng là lệnh vừa qua bộ trọng tài. Dấu thời gian lấy theo cycle của control loop
// chứ không phải giờ hệ thống lúc gọi: host loại lệnh quá hạn, nên control loop treo phải làm dấu
// thời gian đứng yên để host thấy được và ngừng phát.
// chứ không phải giờ hệ thống lúc gọi: host loại lệnh quá hạn (cửa 0.05 s, amr_publiser.cpp:361),
// nên control loop treo phải làm dấu thời gian đứng yên để host thấy được và ngừng phát.
//
// Và khi KHÔNG có yêu cầu nào, dấu thời gian cũng phải đứng yên — đóng dấu mọi cycle là host
// phát 0 lên /cmd_vel ở 20 Hz vĩnh viễn, đè chết teleop/joystick (rqt_robot_steering 10 Hz thua
// 2:1 — robot chỉ nhích rồi đứng im). Bản cũ chỉ đóng dấu trong executeCycle và ở
// publishZeroVelocity(), tức chỉ khi đang điều khiển; đây là cùng ngữ nghĩa. Giữ thêm một cửa
// ân hạn sau khi yêu cầu kết thúc để lệnh dừng cuối cùng chắc chắn lọt qua cửa 0.05 s của host
// — kết thúc mà đóng băng ngay thì robot giữ nguyên vận tốc chót vì lệnh 0 không bao giờ được
// publish.
constexpr double kStopGraceSeconds = 0.5; // [s] ~10 cửa sổ 0.05s của host
const robot_geometry_msgs::Twist& command = loop_.lastCommand();
const robot::Time cycle_time = loop_.lastCycleTime();
if (loop_.hasActiveRequest())
{
last_command_active_time_ = cycle_time;
}
const bool commanding =
loop_.hasActiveRequest() ||
(!last_command_active_time_.isZero() &&
(cycle_time - last_command_active_time_).toSec() < kStopGraceSeconds);
std::lock_guard<std::mutex> lock(data_mutex_);
twist_.velocity = robot_nav_2d_utils::twist3Dto2D(command);
twist_.header.stamp = loop_.lastCycleTime();
twist_.header.frame_id = robot_base_frame_;
if (commanding)
{
twist_.header.stamp = cycle_time;
}
// Không commanding: giữ nguyên stamp cũ — host thấy lệnh quá hạn và nhường /cmd_vel cho nguồn khác.
}
robot::move_base_core::State NavigationServer::toHostState(NavigationState state)
@@ -187,14 +279,99 @@ void NavigationServer::initialize(robot::TFListenerPtr tf)
{
tf_ = tf;
// Phần dựng costmap, planner runner, controller runner và recovery runner từ tf này thuộc bước
// nối dây runtime. Cho tới lúc đó, các cổng phải được bơm vào qua configureLoop() — cố ý KHÔNG
// tự dựng cổng giả ở đây, vì một runtime chạy được với cổng giả là thứ nguy hiểm nhất có thể có.
if (!loop_.initialized())
if (loop_.initialized())
{
nav_feedback_->is_ready = false;
nav_feedback_->feed_back_str = "đã nhận tf, chờ configureLoop() nạp các cổng runtime";
// Cổng đã được bơm vào từ ngoài (test, hoặc host tự lắp). Không dựng đè lên.
return;
}
runtime_.reset(new NavigationRuntime());
std::string error;
// --- Pha 1: config + costmap. CHƯA nạp plugin nào. -------------------------------------------
if (!runtime_->buildCostmaps(tf_, error))
{
runtime_.reset();
nav_feedback_->is_ready = false;
nav_feedback_->feed_back_str = "không dựng được costmap: " + error;
robot::log_error("[move_base2] initialize() thất bại: %s\n", error.c_str());
return;
}
if (!configureSensors(runtime_->config().sensors, error))
{
runtime_.reset();
nav_feedback_->is_ready = false;
nav_feedback_->feed_back_str = "cấu hình cảm biến lỗi: " + error;
return;
}
// --- Gắn costmap và PHÁT LẠI static map, TRƯỚC khi nạp planner -------------------------------
//
// Thứ tự này bắt buộc. `SBPLLatticePlanner::initialize` chặn cho tới khi costmap có kích thước
// khác 0 (`sbpl_lattice_planner.cpp:132`), mà kích thước đó chỉ được đặt khi static map tới
// `StaticLayer`. Nạp planner trước khi map vào được costmap là khoá chết: planner chờ map, map
// chờ planner xong. SBPL `exit(1)` sau 2 giây và giết cả tiến trình.
//
// Bản cũ đi đúng thứ tự này: dựng costmap (`move_base.cpp:260`) -> đẩy map (`:287`) -> nạp
// planner (`:306`).
// Frame của hai lưới lấy TỪ chính costmap, không đoán: global thường là `map`, local là `odom`,
// nhưng cả hai đều do config quyết định.
global_frame_ = runtime_->globalCostmap()->getGlobalFrameID();
local_frame_ = runtime_->localCostmap()->getGlobalFrameID();
attachCostmaps(runtime_->globalCostmap()->getLayeredCostmap(),
runtime_->localCostmap()->getLayeredCostmap());
// --- Pha 2: nạp planner, controller, recovery, action ----------------------------------------
if (!runtime_->buildRunners(error))
{
runtime_.reset();
nav_feedback_->is_ready = false;
nav_feedback_->feed_back_str = "không nạp được runtime: " + error;
robot::log_error("[move_base2] initialize() thất bại: %s\n", error.c_str());
return;
}
if (!configureLoop(runtime_->config().toControlLoopConfig(), runtime_->deps(), error))
{
runtime_.reset();
robot::log_error("[move_base2] initialize() không cấu hình được control loop: %s\n",
error.c_str());
return;
}
// Mission layer đẩy chặng xuống qua callback này. Callback chạy trên CONTROL THREAD — bridge cất
// chặng lại và chỉ nhả ra ở pumpPendingRequest(), nên submit() không bị gọi từ thread mission.
runtime_->mission().setRequestCallback([this](const NavigationRequest& request) {
std::string reason;
if (!loop_.submit(request, reason))
{
last_reject_reason_ = reason;
robot::log_error("[move_base2] từ chối chặng mission %llu: %s\n",
static_cast<unsigned long long>(request.mission_sequence_id),
reason.c_str());
}
});
runtime_->mission().setCancelCallback([this]() { cancel(); });
// start() sau cùng: cho thread cập nhật costmap chạy khi mọi thứ khác đã lắp xong.
runtime_->start();
// Rồi mới cho control loop chạy. Đây là thứ biến một tập thành phần đã lắp thành một runtime
// đang chạy — thiếu nó thì goal chỉ nằm trong chỗ chờ.
if (!startControlThread(runtime_->config().controller_frequency))
{
runtime_.reset();
nav_feedback_->is_ready = false;
nav_feedback_->feed_back_str = "không khởi động được control thread";
return;
}
nav_feedback_->is_ready = true;
nav_feedback_->feed_back_str = "sẵn sàng";
refreshFeedback();
}
// ================================================================================================
@@ -497,19 +674,60 @@ bool NavigationServer::rotateTo(const robot_geometry_msgs::PoseStamped& goal,
// Điều khiển vòng đời
// ================================================================================================
// Ba hàm dưới đây được host gọi từ thread OPC-UA và VDA5050 (amr_control.cpp:159, 184), KHÔNG phải
// từ control thread. `ControlLoop` tự khai là không thread-safe, và `requestPause()` còn ghi hai cờ
// không nguyên tử — xen kẽ với `requestResume()` có thể để lại cả hai cùng false. Vì vậy ở đây chỉ
// ghi nhận yêu cầu dưới `data_mutex_`; `spinOnce()` chuyển tiếp xuống lõi trên control thread.
void NavigationServer::pause()
{
loop_.requestPause();
std::lock_guard<std::mutex> lock(data_mutex_);
pause_requested_ = true;
resume_requested_ = false; // Hai cờ đối nghịch, đặt cùng nhau dưới một lock.
}
void NavigationServer::resume()
{
loop_.requestResume();
std::lock_guard<std::mutex> lock(data_mutex_);
resume_requested_ = true;
pause_requested_ = false;
}
void NavigationServer::cancel()
{
loop_.requestCancel();
std::lock_guard<std::mutex> lock(data_mutex_);
cancel_requested_ = true;
}
void NavigationServer::drainLifecycleRequests()
{
bool pause = false;
bool resume = false;
bool cancel = false;
{
std::lock_guard<std::mutex> lock(data_mutex_);
pause = pause_requested_;
resume = resume_requested_;
cancel = cancel_requested_;
pause_requested_ = false;
resume_requested_ = false;
cancel_requested_ = false;
}
// Huỷ trước: nó thắng mọi thứ khác. Tạm dừng rồi huỷ và huỷ rồi tạm dừng phải cho cùng kết quả.
if (cancel)
{
loop_.requestCancel();
}
if (pause)
{
loop_.requestPause();
}
if (resume)
{
loop_.requestResume();
}
}
bool NavigationServer::setTwistLinear(const robot_geometry_msgs::Vector3& linear)
@@ -640,14 +858,95 @@ robot::move_base_core::NavFeedback* NavigationServer::getFeedback()
return nav_feedback_.get();
}
// `getGlobalData`/`getLocalData` KHÔNG phải getter thuần: bản cũ làm việc thật bên trong chúng
// (điền plan, chuyển costmap sang OccupancyGrid, cập nhật footprint) và host gọi từ bốn ros::Timer
// riêng để publish lên rviz — đây là toàn bộ đường xuất visualization.
//
// Đã truy hết consumer: **chỉ rviz**. OPC-UA và VDA5050 không đụng tới, không có gì trong control
// loop hay mission path đọc chúng. Vì vậy phần plan và footprint được điền ở đây (rẻ, không cần
// chuyển đổi gì), còn phần lưới costmap -> OccupancyGrid vẫn để trống — xem @warning ở header.
void NavigationServer::cachePlans()
{
// Chạy trên CONTROL THREAD. Đây là chỗ duy nhất chạm vào controller để lấy quỹ đạo cục bộ —
// `ControllerPort` không thread-safe và plugin (Priest) lại càng không. Gọi nó từ ros::Timer của
// host là chạm plugin song song với lúc control thread đang tính lệnh trên chính plugin đó.
// CHỈ hỏi khi controller thật sự đang bám plan. Hỏi mỗi cycle kể cả lúc rảnh nghĩa là gọi vào
// plugin 30 lần/giây để nhận về "chưa có gì" — PriestLocalPlanner log cảnh báo mỗi lần, và đó
// đúng là kiểu spam trong control loop mà quy ước của repo cấm. Không ở kControlling thì cũng
// không có quỹ đạo cục bộ nào để hiển thị.
robot_nav_2d_msgs::Path2D local;
ControllerPort* controller = loop_.controllerPort();
if (controller != nullptr && loop_.state() == NavigationState::kControlling)
{
controller->getLocalPlan(local);
}
robot_nav_2d_msgs::Path2D global = robot_nav_2d_utils::posesToPath2D(loop_.latestPlan());
// Đóng dấu bằng đồng hồ control loop: host bỏ qua plan cũ hơn 0.5 s và plan có dấu bằng 0
// (amr_publiser.cpp:481-495). Loop treo thì dấu đứng yên và host ngừng publish — đúng ý.
const robot::Time stamp = loop_.lastCycleTime();
global.header.stamp = stamp;
local.header.stamp = stamp;
if (global.header.frame_id.empty())
{
global.header.frame_id = global_frame_;
}
if (local.header.frame_id.empty())
{
local.header.frame_id = local_frame_;
}
std::lock_guard<std::mutex> lock(data_mutex_);
cached_global_plan_ = std::move(global);
cached_local_plan_ = std::move(local);
}
robot::move_base_core::PlannerDataOutput NavigationServer::buildPlannerData(CostmapExporter* exporter,
bool local_plan)
{
// Dựng vào một BẢN CỤC BỘ rồi trả về theo giá trị. Ghi thẳng vào `global_data_`/`local_data_` là
// hỏng thật: host gọi getGlobalData() và getLocalData() từ BỐN ros::Timer khác nhau, và hai timer
// dùng chung một thành viên sẽ ghi đè nhau giữa chừng — đã gây `std::bad_alloc` rồi
// `malloc(): invalid size` và giết cả tiến trình.
robot::move_base_core::PlannerDataOutput data;
{
std::lock_guard<std::mutex> lock(data_mutex_);
data.plan = local_plan ? cached_local_plan_ : cached_global_plan_;
data.footprint.header.frame_id = robot_base_frame_;
data.footprint.polygon.points.reserve(footprint_.size());
for (const auto& point : footprint_)
{
robot_geometry_msgs::Point32 p32;
p32.x = static_cast<float>(point.x);
p32.y = static_cast<float>(point.y);
p32.z = static_cast<float>(point.z);
data.footprint.polygon.points.push_back(p32);
}
}
if (exporter != nullptr)
{
// `CostmapExporter` tự khoá bên trong, nên gọi từ nhiều thread là an toàn.
exporter->fill(data.costmap, data.costmap_update, data.is_costmap_updated);
}
return data;
}
robot::move_base_core::PlannerDataOutput NavigationServer::getGlobalData()
{
return global_data_;
return buildPlannerData(runtime_ != nullptr ? &runtime_->globalExporter() : nullptr,
/*local_plan=*/false);
}
robot::move_base_core::PlannerDataOutput NavigationServer::getLocalData()
{
return local_data_;
return buildPlannerData(runtime_ != nullptr ? &runtime_->localExporter() : nullptr,
/*local_plan=*/true);
}
} // namespace move_base2

137
src/runners/controller_runner.cpp
View File

@@ -17,6 +17,8 @@
#include <robot/plugin_loader_helper.h>
#include <robot/robot.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_nav_2d_utils/conversions.h>
namespace move_base2
{
@@ -37,8 +39,9 @@ bool isFiniteTwist(const robot_geometry_msgs::Twist& twist)
ControllerRunner::ControllerRunner() = default;
ControllerRunner::~ControllerRunner() = default;
bool ControllerRunner::configure(const robot::NodeHandle& nh, tf3::BufferCore* tf,
robot_costmap_2d::Costmap2DROBOT* costmap,
bool ControllerRunner::configure(const robot::NodeHandle& nh,
const std::shared_ptr<tf3::BufferCore>& tf,
robot_costmap_2d::Costmap2DROBOT* costmap, const PosePort* pose,
const std::string& initial_controller, std::string& error)
{
if (configured_)
@@ -53,9 +56,18 @@ bool ControllerRunner::configure(const robot::NodeHandle& nh, tf3::BufferCore* t
return false;
}
if (pose == nullptr)
{
// Gen-2 nhận pose làm THAM SỐ của computeVelocityCommands/isGoalReached; không có nguồn pose
// thì không gọi được hàm nào trong hai hàm đó.
error = "ControllerRunner cần PosePort khác null";
return false;
}
nh_ = nh;
tf_ = tf;
costmap_ = costmap;
pose_ = pose;
configured_ = true;
if (!initial_controller.empty() && !swapPlanner(initial_controller))
@@ -68,7 +80,7 @@ bool ControllerRunner::configure(const robot::NodeHandle& nh, tf3::BufferCore* t
return true;
}
robot_nav_core::BaseLocalPlanner* ControllerRunner::acquire(const std::string& name)
robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name)
{
const auto cached = controllers_.find(name);
if (cached != controllers_.end())
@@ -91,7 +103,7 @@ robot_nav_core::BaseLocalPlanner* ControllerRunner::acquire(const std::string& n
try
{
loaded.factory = boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
loaded.factory = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
library_path, name, boost::dll::load_mode::append_decorations);
}
catch (const boost::system::system_error& ex)
@@ -126,9 +138,9 @@ robot_nav_core::BaseLocalPlanner* ControllerRunner::acquire(const std::string& n
try
{
// Khác BaseGlobalPlanner: initialize ở đây trả void, nên không có cách nào biết plugin tự thấy
// mình hỏng. Chỉ chặn được exception.
loaded.instance->initialize(name, tf_, costmap_);
// Chữ ký gen-2: (parent NodeHandle, tên, TF, costmap). Khác hẳn gen-1 — và vì Boost.DLL không
// kiểm kiểu, gọi nhầm chữ ký sẽ không lỗi biên dịch mà hỏng vtable lúc chạy.
loaded.instance->initialize(nh_, name, tf_, costmap_);
}
catch (const std::exception& ex)
{
@@ -141,7 +153,7 @@ robot_nav_core::BaseLocalPlanner* ControllerRunner::acquire(const std::string& n
return inserted.first->second.instance.get();
}
void ControllerRunner::applyPendingLimits(robot_nav_core::BaseLocalPlanner* controller)
void ControllerRunner::applyPendingLimits(robot_nav_core2::LocalPlanner* controller)
{
if (controller == nullptr)
{
@@ -191,7 +203,7 @@ bool ControllerRunner::swapPlanner(const std::string& planner_name)
return true; // Đã đúng controller; không log để khỏi spam ở cửa vào mỗi yêu cầu.
}
robot_nav_core::BaseLocalPlanner* controller = acquire(planner_name);
robot_nav_core2::LocalPlanner* controller = acquire(planner_name);
if (controller == nullptr)
{
// Giữ nguyên controller đang chạy: bên gọi từ chối yêu cầu dựa vào giá trị trả về.
@@ -200,6 +212,7 @@ bool ControllerRunner::swapPlanner(const std::string& planner_name)
active_ = controller;
active_name_ = planner_name;
has_active_goal_ = false; // Instance mới chưa biết goal nào.
applyPendingLimits(active_);
robot::log_info("[move_base2] ControllerRunner: local planner đang dùng là '%s'.\n",
@@ -240,7 +253,23 @@ bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStampe
try
{
return active_->setPlan(plan);
// Gen-2 tách goal khỏi plan: `setGoalPose` phải gọi TRƯỚC `setPlan`, đúng như
// `LocalPlannerAdapter` làm. Goal là pose CUỐI của plan.
const robot_nav_2d_msgs::Path2D path = robot_nav_2d_utils::posesToPath2D(plan);
if (path.poses.empty())
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: plan chuyển sang Path2D bị rỗng.\n");
return false;
}
// `Path2D::poses` đã là vector<Pose2DStamped>, nên pose cuối dùng thẳng làm goal.
const robot_nav_2d_msgs::Pose2DStamped goal_pose = path.poses.back();
active_->setGoalPose(goal_pose);
active_->setPlan(path);
has_active_goal_ = true;
return true;
}
catch (const std::exception& ex)
{
@@ -251,6 +280,21 @@ bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStampe
}
}
bool ControllerRunner::currentPose(robot_nav_2d_msgs::Pose2DStamped& pose) const
{
if (pose_ == nullptr)
{
return false;
}
robot_geometry_msgs::PoseStamped stamped;
if (!pose_->getRobotPose(stamped))
{
return false;
}
pose = robot_nav_2d_utils::poseStampedToPose2D(stamped);
return true;
}
bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
{
cmd = robot_geometry_msgs::Twist();
@@ -263,23 +307,35 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
return false;
}
if (!has_active_goal_)
{
// Chưa có plan nào được nạp. Không phải lỗi — chỉ là chưa tới lúc tính lệnh.
return false;
}
robot_nav_2d_msgs::Pose2DStamped pose;
if (!currentPose(pose))
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: mất pose, không tính lệnh.\n");
return false;
}
robot_geometry_msgs::Twist result;
bool ok = false;
try
{
ok = active_->computeVelocityCommands(measured_velocity_, result);
// Gen-2 trả THẲNG lệnh (không có cờ thành công/thất bại) và ném exception khi không tính được —
// ngược với gen-1. Vì vậy nhánh "không có lệnh hợp lệ" ở đây là nhánh catch.
const robot_nav_2d_msgs::Twist2DStamped cmd_2d =
active_->computeVelocityCommands(pose, robot_nav_2d_utils::twist3Dto2D(measured_velocity_));
result = robot_nav_2d_utils::twist2Dto3D(cmd_2d.velocity);
}
catch (const std::exception& ex)
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: '%s' ném exception khi tính lệnh: "
"%s\n", active_name_.c_str(), ex.what());
return false;
}
if (!ok)
{
"[move_base2] ControllerRunner: '%s' không sinh được lệnh: %s\n",
active_name_.c_str(), ex.what());
return false;
}
@@ -304,9 +360,26 @@ bool ControllerRunner::isGoalReached()
return false;
}
if (!has_active_goal_)
{
return false;
}
robot_nav_2d_msgs::Pose2DStamped pose;
if (!currentPose(pose))
{
return false; // Mất pose: "chưa tới đích" là phía an toàn.
}
try
{
return active_->isGoalReached();
const bool reached =
active_->isGoalReached(pose, robot_nav_2d_utils::twist3Dto2D(measured_velocity_));
if (reached)
{
has_active_goal_ = false;
}
return reached;
}
catch (const std::exception& ex)
{
@@ -319,6 +392,30 @@ bool ControllerRunner::isGoalReached()
}
}
void ControllerRunner::getLocalPlan(robot_nav_2d_msgs::Path2D& plan)
{
plan = robot_nav_2d_msgs::Path2D();
if (!configured_ || active_ == nullptr)
{
return;
}
try
{
active_->getPlan(plan);
}
catch (const std::exception& ex)
{
// Không phải mọi planner đều hỗ trợ; gen-2 cho phép ném. Đây chỉ là dữ liệu hiển thị nên nuốt
// exception là đúng — nhưng vẫn log để không ai tưởng rviz đang hiện quỹ đạo thật.
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: '%s' không trả được quỹ đạo cục bộ: "
"%s\n", active_name_.c_str(), ex.what());
plan = robot_nav_2d_msgs::Path2D();
}
}
void ControllerRunner::setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity)
{
if (!isFiniteTwist(velocity))

126
src/state_machine.cpp
View File

@@ -158,6 +158,68 @@ void StateMachine::finish(NavigationState terminal, NavigationOutcome outcome,
enter(terminal, now, reason, out);
}
void StateMachine::acceptPendingRequest(const StateMachineInput& in, StateMachineOutput& out)
{
out.accept_request = true;
recovery_index_ = 0;
request_has_goal_ = in.pending_request_has_goal;
action_count_ = in.pending_request_action_count;
action_index_ = 0;
if (!request_has_goal_)
{
// D8: yêu cầu chỉ-có-action — không có gì để lập plan, vào thẳng thực thi action.
if (action_count_ == 0)
{
// Không goal lẫn action là vi phạm contract; mission layer đã validate nhưng lõi vẫn phải tự
// vệ: kết thúc tường minh thay vì treo ở một state không có đường ra.
finish(NavigationState::kAborted, NavigationOutcome::kFailed, in.now,
"yêu cầu không có goal lẫn action", out);
return;
}
out.start_action = true;
out.action_index = 0;
action_started_at_ = in.now;
enter(NavigationState::kExecutingActions, in.now, "yêu cầu chỉ có action", out);
return;
}
out.start_planner = true;
beginPlanningCycle(in.now);
last_valid_control_ = in.now;
last_oscillation_reset_ = in.now;
out.reset_oscillation_origin = true;
enter(NavigationState::kPlanning, in.now, "nhận yêu cầu mới", out);
}
bool StateMachine::preemptIfRequested(const StateMachineInput& in, StateMachineOutput& out)
{
if (!in.has_pending_request)
{
return false;
}
// Dọn sạch yêu cầu cũ TRƯỚC khi nhận yêu cầu mới: planner đang chạy phải bị huỷ (kết quả của nó
// thuộc goal cũ), recovery và action đang chạy phải được bảo dừng an toàn.
out.stop_planner = true;
if (state_ == NavigationState::kRecovering)
{
out.cancel_recovery = true;
}
if (state_ == NavigationState::kExecutingActions)
{
out.cancel_action = true;
}
// Báo kết quả cho chặng cũ ĐÚNG MỘT LẦN, ngay tại đây. Không báo thì mission layer chờ vĩnh viễn
// một chặng đã bị thay.
out.report_outcome = true;
out.outcome = NavigationOutcome::kPreempted;
acceptPendingRequest(in, out);
return true;
}
StateMachineOutput StateMachine::update(const StateMachineInput& in)
{
StateMachineOutput out;
@@ -193,36 +255,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
{
if (in.has_pending_request)
{
out.accept_request = true;
recovery_index_ = 0;
request_has_goal_ = in.pending_request_has_goal;
action_count_ = in.pending_request_action_count;
action_index_ = 0;
if (!request_has_goal_)
{
// D8: yêu cầu chỉ-có-action — không có gì để lập plan, vào thẳng thực thi action.
if (action_count_ == 0)
{
// Không goal lẫn action là vi phạm contract; mission layer đã validate nhưng lõi vẫn
// phải tự vệ: kết thúc tường minh thay vì treo ở một state không có đường ra.
finish(NavigationState::kAborted, NavigationOutcome::kFailed, in.now,
"yêu cầu không có goal lẫn action", out);
break;
}
out.start_action = true;
out.action_index = 0;
action_started_at_ = in.now;
enter(NavigationState::kExecutingActions, in.now, "yêu cầu chỉ có action", out);
break;
}
out.start_planner = true;
beginPlanningCycle(in.now);
last_valid_control_ = in.now;
last_oscillation_reset_ = in.now;
out.reset_oscillation_origin = true;
enter(NavigationState::kPlanning, in.now, "nhận yêu cầu mới", out);
acceptPendingRequest(in, out);
}
break;
}
@@ -244,6 +277,12 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
break;
}
// Goal mới chen ngang: thay ngay, không xếp hàng.
if (preemptIfRequested(in, out))
{
break;
}
if (in.planner == PlannerFeedback::kPlanReady)
{
out.apply_plan = true;
@@ -299,6 +338,12 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
break;
}
// Goal mới chen ngang: thay ngay, không xếp hàng.
if (preemptIfRequested(in, out))
{
break;
}
// Plan mới tới giữa lúc đang bám plan cũ: nhận ngay, vẫn ở kControlling.
if (in.planner == PlannerFeedback::kPlanReady)
{
@@ -379,6 +424,13 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
// --------------------------------------------------------------------------------------
case NavigationState::kRecovering:
{
// Goal mới chen ngang: thay ngay, không xếp hàng. Đặt TRƯỚC mọi xử lý khác của state này —
// yêu cầu cũ đã bị thay thì mọi quyết định dựa trên nó đều vô nghĩa.
if (preemptIfRequested(in, out))
{
break;
}
if (in.cancel_requested)
{
out.cancel_recovery = true;
@@ -417,6 +469,13 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
// --------------------------------------------------------------------------------------
case NavigationState::kExecutingActions:
{
// Goal mới chen ngang: thay ngay, không xếp hàng. Đặt TRƯỚC mọi xử lý khác của state này —
// yêu cầu cũ đã bị thay thì mọi quyết định dựa trên nó đều vô nghĩa.
if (preemptIfRequested(in, out))
{
break;
}
if (in.cancel_requested)
{
out.cancel_action = true;
@@ -478,6 +537,13 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
// --------------------------------------------------------------------------------------
case NavigationState::kPaused:
{
// Goal mới chen ngang: thay ngay, không xếp hàng. Đặt TRƯỚC mọi xử lý khác của state này —
// yêu cầu cũ đã bị thay thì mọi quyết định dựa trên nó đều vô nghĩa.
if (preemptIfRequested(in, out))
{
break;
}
if (in.cancel_requested)
{
enter(NavigationState::kCancelling, in.now, "huỷ khi đang tạm dừng", out);