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

optimal & fix file cmake

Browse Source
This commit is contained in:
duongtd
2026-08-03 22:41:32 +07:00
parent d8babff20b
commit 701d25f952
70 changed files with 5572 additions and 1146 deletions
Download Patch File Download Diff File Expand all files Collapse all files

254
src/runners/controller_runner.cpp
View File

@@ -10,6 +10,7 @@
#include <cmath>
#include <exception>
#include <sstream>
#include <utility>
#include <boost/dll/import.hpp>
@@ -39,6 +40,17 @@ bool isFiniteTwist(const robot_geometry_msgs::Twist& twist)
ControllerRunner::ControllerRunner() = default;
ControllerRunner::~ControllerRunner() = default;
void ControllerRunner::attachStats(RuntimeStats* stats)
{
stats_ = stats;
if (stats_ == nullptr)
{
return;
}
section_compute_ = stats_->section("controller.compute");
section_local_plan_ = stats_->section("controller.getLocalPlan");
}
bool ControllerRunner::configure(const robot::NodeHandle& nh,
const std::shared_ptr<tf3::BufferCore>& tf,
robot_costmap_2d::Costmap2DROBOT* costmap, const PosePort* pose,
@@ -46,13 +58,13 @@ bool ControllerRunner::configure(const robot::NodeHandle& nh,
{
if (configured_)
{
error = "ControllerRunner::configure() gọi lần thứ hai";
error = "ControllerRunner::configure() called twice";
return false;
}
if (costmap == nullptr)
{
error = "ControllerRunner cần costmap local khác null";
error = "ControllerRunner needs a non-null local costmap";
return false;
}
@@ -60,7 +72,7 @@ bool ControllerRunner::configure(const robot::NodeHandle& nh,
{
// 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";
error = "ControllerRunner needs a non-null PosePort";
return false;
}
@@ -72,7 +84,7 @@ bool ControllerRunner::configure(const robot::NodeHandle& nh,
if (!initial_controller.empty() && !swapPlanner(initial_controller))
{
error = "không nạp được local planner khởi đầu '" + initial_controller + "'";
error = "could not load the initial local planner '" + initial_controller + "'";
configured_ = false;
return false;
}
@@ -85,6 +97,35 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
const auto cached = controllers_.find(name);
if (cached != controllers_.end())
{
if (!marker_dirty_)
{
return cached->second.instance.get();
}
// Marker vừa đổi: instance này có thể đã đọc `maker_name` trong initialize() và không bao giờ
// đọc lại. Dựng lại từ factory sẵn có (không dlopen lại); instance mới thay chỗ instance cũ
// CHỈ khi initialize() thành công — thất bại thì giữ nguyên cache và trả lỗi để bên gọi từ
// chối yêu cầu, không để lại trạng thái nửa vời.
try
{
robot_nav_core2::LocalPlanner::Ptr fresh = cached->second.factory();
if (!fresh)
{
robot::log_error("[move_base2] ControllerRunner: factory of '%s' returned nullptr while "
"rebuilding for the new marker.\n", name.c_str());
return nullptr;
}
fresh->initialize(nh_, name, tf_, costmap_);
cached->second.instance = std::move(fresh);
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ControllerRunner: rebuilding '%s' for the new marker failed: "
"%s\n",
name.c_str(), ex.what());
return nullptr;
}
marker_dirty_ = false;
return cached->second.instance.get();
}
@@ -93,8 +134,8 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
if (library_path.empty())
{
robot::log_error("[move_base2] ControllerRunner: không tìm được thư viện cho '%s' — kiểm khoá "
"'%s/library_path' trong YAML và sự tồn tại của file .so trong devel/lib.\n",
robot::log_error("[move_base2] ControllerRunner: no library found for '%s' — check the key "
"'%s/library_path' in the YAML and that the .so file exists in devel/lib.\n",
name.c_str(), name.c_str());
return nullptr;
}
@@ -108,13 +149,13 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
}
catch (const boost::system::system_error& ex)
{
robot::log_error("[move_base2] ControllerRunner: không nạp được symbol '%s' từ '%s': %s\n",
robot::log_error("[move_base2] ControllerRunner: could not load symbol '%s' from '%s': %s\n",
name.c_str(), library_path.c_str(), ex.what());
return nullptr;
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ControllerRunner: lỗi khi nạp '%s': %s\n", name.c_str(),
robot::log_error("[move_base2] ControllerRunner: error while loading '%s': %s\n", name.c_str(),
ex.what());
return nullptr;
}
@@ -125,14 +166,15 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ControllerRunner: factory của '%s' ném exception: %s\n",
robot::log_error("[move_base2] ControllerRunner: factory of '%s' threw an exception: %s\n",
name.c_str(), ex.what());
return nullptr;
}
if (!loaded.instance)
{
robot::log_error("[move_base2] ControllerRunner: factory của '%s' trả nullptr.\n", name.c_str());
robot::log_error("[move_base2] ControllerRunner: factory of '%s' returned nullptr.\n",
name.c_str());
return nullptr;
}
@@ -144,15 +186,59 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ControllerRunner: initialize() của '%s' ném exception: %s\n",
robot::log_error("[move_base2] ControllerRunner: initialize() of '%s' threw an exception: %s\n",
name.c_str(), ex.what());
return nullptr;
}
const auto inserted = controllers_.emplace(name, std::move(loaded));
// Instance mới vừa initialize() với `maker_name` hiện hành — marker không còn "chưa được đọc".
marker_dirty_ = false;
return inserted.first->second.instance.get();
}
bool ControllerRunner::setDockingMarker(const std::string& marker)
{
if (!configured_)
{
robot::log_error("[move_base2] ControllerRunner: setDockingMarker() before configure().\n");
return false;
}
// Validate với danh sách `maker_sources` (chuỗi cách nhau bằng space, maker_sources.yaml) —
// đúng phép kiểm bản cũ làm ở cửa dockTo (move_base.cpp:1161-1173). Marker lạ phải bị chặn ở
// đây: để lọt xuống thì getMaker() của docking planner âm thầm không match source nào và robot
// đứng im không lý do.
std::string sources;
nh_.param("maker_sources", sources, std::string(""));
std::stringstream ss(sources);
std::string source;
bool known = false;
while (ss >> source)
{
if (source == marker)
{
known = true;
break;
}
}
if (!known)
{
robot::log_error("[move_base2] ControllerRunner: marker '%s' is not listed in maker_sources "
"('%s').\n", marker.c_str(), sources.c_str());
return false;
}
std::string current;
nh_.param("maker_name", current, std::string(""));
if (current != marker)
{
nh_.setParam("maker_name", marker);
marker_dirty_ = true;
}
return true;
}
void ControllerRunner::applyPendingLimits(robot_nav_core2::LocalPlanner* controller)
{
if (controller == nullptr)
@@ -180,7 +266,8 @@ void ControllerRunner::applyPendingLimits(robot_nav_core2::LocalPlanner* control
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ControllerRunner: lỗi khi áp lại trần vận tốc: %s\n", ex.what());
robot::log_error("[move_base2] ControllerRunner: error while re-applying the velocity limits: "
"%s\n", ex.what());
}
}
@@ -188,19 +275,22 @@ bool ControllerRunner::swapPlanner(const std::string& planner_name)
{
if (!configured_)
{
robot::log_error("[move_base2] ControllerRunner: swapPlanner() trước configure().\n");
robot::log_error("[move_base2] ControllerRunner: swapPlanner() before configure().\n");
return false;
}
if (planner_name.empty())
{
robot::log_error("[move_base2] ControllerRunner: tên controller rỗng.\n");
robot::log_error("[move_base2] ControllerRunner: empty controller name.\n");
return false;
}
if (planner_name == active_name_ && active_ != nullptr)
if (planner_name == active_name_ && active_ != nullptr && !marker_dirty_)
{
return true; // Đã đúng controller; không log để khỏi spam ở cửa vào mỗi yêu cầu.
// Đã đúng controller; không log để khỏi spam ở cửa vào mỗi yêu cầu. Riêng khi marker vừa đổi
// thì KHÔNG được đi tắt: dock lại cùng planner với marker khác phải rơi xuống acquire() để
// instance được dựng lại và initialize() đọc `maker_name` mới.
return true;
}
robot_nav_core2::LocalPlanner* controller = acquire(planner_name);
@@ -213,33 +303,21 @@ 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.
active_plan_.clear();
applyPendingLimits(active_);
robot::log_info("[move_base2] ControllerRunner: local planner đang dùng là '%s'.\n",
robot::log_info("[move_base2] ControllerRunner: active local planner is '%s'.\n",
planner_name.c_str());
return true;
}
void ControllerRunner::setTolerance(double xy_m, double yaw_rad)
{
if (!configured_)
{
return;
}
// Interface gen-1 không có hàm đặt sai số; bản cũ ghi vào param rồi để planner tự đọc lại. Kênh
// gián tiếp này được giữ nguyên để không đổi hành vi của các planner đang chạy — nhưng planner
// nào chỉ đọc param lúc initialize sẽ KHÔNG thấy giá trị mới. Xem doc của lớp.
nh_.setParam("xy_goal_tolerance", xy_m);
nh_.setParam("yaw_goal_tolerance", yaw_rad);
}
bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan)
{
if (!configured_ || active_ == nullptr)
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: setPlan() khi chưa có controller.\n");
"[move_base2] ControllerRunner: setPlan() with no controller "
"loaded.\n");
return false;
}
@@ -247,7 +325,7 @@ bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStampe
{
// Plan rỗng lọt xuống sẽ thành front()/back() trên vector rỗng bên trong planner.
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: từ chối plan rỗng.\n");
"[move_base2] ControllerRunner: rejecting an empty plan.\n");
return false;
}
@@ -259,7 +337,8 @@ bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStampe
if (path.poses.empty())
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: plan chuyển sang Path2D bị rỗng.\n");
"[move_base2] ControllerRunner: plan converted to Path2D came out "
"empty.\n");
return false;
}
@@ -269,17 +348,84 @@ bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStampe
active_->setGoalPose(goal_pose);
active_->setPlan(path);
has_active_goal_ = true;
active_plan_ = plan;
return true;
}
catch (const std::exception& ex)
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: '%s' ném exception trong setPlan: "
"[move_base2] ControllerRunner: '%s' threw an exception in setPlan: "
"%s\n", active_name_.c_str(), ex.what());
return false;
}
}
bool ControllerRunner::refreshActivePlanner()
{
if (!configured_ || active_ == nullptr)
{
// Chưa có controller active thì không có cache nào cần refresh. Hành vi này giúp footprint có
// thể được host đặt trước goal đầu tiên mà không biến thành lỗi khởi tạo.
return true;
}
const auto loaded = controllers_.find(active_name_);
if (loaded == controllers_.end())
{
robot::log_error("[move_base2] ControllerRunner: active local planner '%s' is absent from "
"the plugin cache.\n", active_name_.c_str());
return false;
}
const bool had_active_goal = has_active_goal_;
const std::vector<robot_geometry_msgs::PoseStamped> saved_plan = active_plan_;
if (had_active_goal && saved_plan.empty())
{
// Không thay instance cũ nếu không thể khôi phục goal đang chạy. Giữ controller hiện tại vẫn
// an toàn hơn việc âm thầm biến navigation thành controller không có plan.
robot::log_error("[move_base2] ControllerRunner: active planner '%s' has a goal but no "
"cached plan to restore after a footprint change.\n", active_name_.c_str());
return false;
}
robot_nav_core2::LocalPlanner::Ptr fresh;
try
{
fresh = loaded->second.factory();
if (!fresh)
{
robot::log_error("[move_base2] ControllerRunner: factory of '%s' returned nullptr while "
"refreshing its footprint cache.\n", active_name_.c_str());
return false;
}
fresh->initialize(nh_, active_name_, tf_, costmap_);
}
catch (const std::exception& ex)
{
// Chỉ thay cache SAU initialize thành công, nên lỗi này không làm mất controller cũ.
robot::log_error("[move_base2] ControllerRunner: refreshing '%s' after a footprint change "
"failed: %s\n", active_name_.c_str(), ex.what());
return false;
}
loaded->second.instance = std::move(fresh);
active_ = loaded->second.instance.get();
has_active_goal_ = false;
active_plan_.clear();
applyPendingLimits(active_);
if (had_active_goal && !setPlan(saved_plan))
{
robot::log_error("[move_base2] ControllerRunner: could not restore the active plan after "
"refreshing '%s' for a footprint change.\n", active_name_.c_str());
return false;
}
robot::log_info("[move_base2] ControllerRunner: refreshed '%s' after the local costmap "
"footprint changed.\n", active_name_.c_str());
return true;
}
bool ControllerRunner::currentPose(robot_nav_2d_msgs::Pose2DStamped& pose) const
{
if (pose_ == nullptr)
@@ -302,8 +448,8 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
if (!configured_ || active_ == nullptr)
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: computeVelocityCommands() khi chưa "
"có controller.\n");
"[move_base2] ControllerRunner: computeVelocityCommands() with no "
"controller loaded.\n");
return false;
}
@@ -317,7 +463,8 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
if (!currentPose(pose))
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: mất pose, không tính lệnh.\n");
"[move_base2] ControllerRunner: pose lost, not computing a "
"command.\n");
return false;
}
@@ -325,6 +472,9 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
try
{
// Đo đúng lời gọi vào plugin, không đo cả hàm: phần còn lại (tra pose, kiểm NaN) là chi phí của
// move_base2, còn đây mới là chi phí của local planner đang cấu hình.
ScopedSection timer(stats_, section_compute_);
// 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 =
@@ -334,7 +484,7 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
catch (const std::exception& ex)
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: '%s' không sinh được lệnh: %s\n",
"[move_base2] ControllerRunner: '%s' produced no command: %s\n",
active_name_.c_str(), ex.what());
return false;
}
@@ -344,7 +494,8 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
// VelocityArbiter cũng chặn NaN/Inf, nhưng chặn ngay tại nguồn cho biết ĐÚNG plugin nào đang
// trả dữ liệu hỏng — arbiter chỉ thấy một con số vô nghĩa không rõ từ đâu.
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: '%s' trả lệnh chứa NaN/Inf.\n",
"[move_base2] ControllerRunner: '%s' returned a command containing "
"NaN/Inf.\n",
active_name_.c_str());
return false;
}
@@ -378,6 +529,7 @@ bool ControllerRunner::isGoalReached()
if (reached)
{
has_active_goal_ = false;
active_plan_.clear();
}
return reached;
}
@@ -386,7 +538,7 @@ bool ControllerRunner::isGoalReached()
// Trả false: "chưa tới đích" là phía an toàn — báo nhầm đã tới sẽ kết thúc chặng đường sớm và
// robot dừng ở chỗ không phải đích.
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: '%s' ném exception trong "
"[move_base2] ControllerRunner: '%s' threw an exception in "
"isGoalReached: %s\n", active_name_.c_str(), ex.what());
return false;
}
@@ -403,6 +555,7 @@ void ControllerRunner::getLocalPlan(robot_nav_2d_msgs::Path2D& plan)
try
{
ScopedSection timer(stats_, section_local_plan_);
active_->getPlan(plan);
}
catch (const std::exception& ex)
@@ -410,8 +563,8 @@ void ControllerRunner::getLocalPlan(robot_nav_2d_msgs::Path2D& plan)
// 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());
"[move_base2] ControllerRunner: '%s' could not return a local "
"trajectory: %s\n", active_name_.c_str(), ex.what());
plan = robot_nav_2d_msgs::Path2D();
}
}
@@ -423,7 +576,8 @@ void ControllerRunner::setMeasuredVelocity(const robot_geometry_msgs::Twist& vel
// Giữ giá trị cũ thay vì đưa NaN vào hàm tính lệnh — nhiều local planner dùng nó làm mốc giới
// hạn gia tốc, và NaN ở đó lan ra toàn bộ cost function.
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: bỏ vận tốc đo được chứa NaN/Inf.\n");
"[move_base2] ControllerRunner: dropping a measured velocity "
"containing NaN/Inf.\n");
return;
}
measured_velocity_ = velocity;
@@ -433,7 +587,8 @@ bool ControllerRunner::setTwistLinear(const robot_geometry_msgs::Vector3& linear
{
if (!std::isfinite(linear.x) || !std::isfinite(linear.y) || !std::isfinite(linear.z))
{
robot::log_error("[move_base2] ControllerRunner: trần vận tốc thẳng chứa NaN/Inf, bỏ qua.\n");
robot::log_error("[move_base2] ControllerRunner: linear velocity limit contains NaN/Inf, "
"ignored.\n");
return false;
}
@@ -463,7 +618,8 @@ bool ControllerRunner::setTwistLinear(const robot_geometry_msgs::Vector3& linear
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ControllerRunner: '%s' ném exception trong setTwistLinear: %s\n",
robot::log_error("[move_base2] ControllerRunner: '%s' threw an exception in setTwistLinear: "
"%s\n",
active_name_.c_str(), ex.what());
return false;
}
@@ -473,7 +629,8 @@ bool ControllerRunner::setTwistAngular(const robot_geometry_msgs::Vector3& angul
{
if (!std::isfinite(angular.x) || !std::isfinite(angular.y) || !std::isfinite(angular.z))
{
robot::log_error("[move_base2] ControllerRunner: trần vận tốc góc chứa NaN/Inf, bỏ qua.\n");
robot::log_error("[move_base2] ControllerRunner: angular velocity limit contains NaN/Inf, "
"ignored.\n");
return false;
}
@@ -491,7 +648,8 @@ bool ControllerRunner::setTwistAngular(const robot_geometry_msgs::Vector3& angul
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ControllerRunner: '%s' ném exception trong setTwistAngular: %s\n",
robot::log_error("[move_base2] ControllerRunner: '%s' threw an exception in setTwistAngular: "
"%s\n",
active_name_.c_str(), ex.what());
return false;
}