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

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))