first commit

src/runners/controller_runner.cpp

@@ -0,0 +1,408 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — cài đặt ControllerRunner.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/runners/controller_runner.h>
+
+#include <cmath>
+#include <exception>
+#include <utility>
+
+#include <boost/dll/import.hpp>
+#include <boost/system/system_error.hpp>
+
+#include <robot/plugin_loader_helper.h>
+#include <robot/robot.h>
+
+namespace move_base2
+{
+namespace
+{
+
+/// [s] Giãn cách log cho cảnh báo phát sinh trong đường nóng — nhịp control loop, không được spam.
+constexpr double kHotPathLogThrottle = 5.0;
+
+bool isFiniteTwist(const robot_geometry_msgs::Twist& twist)
+{
+  return std::isfinite(twist.linear.x) && std::isfinite(twist.linear.y) &&
+         std::isfinite(twist.angular.z);
+}
+
+}  // namespace
+
+ControllerRunner::ControllerRunner() = default;
+ControllerRunner::~ControllerRunner() = default;
+
+bool ControllerRunner::configure(const robot::NodeHandle& nh, tf3::BufferCore* tf,
+                                 robot_costmap_2d::Costmap2DROBOT* costmap,
+                                 const std::string& initial_controller, std::string& error)
+{
+  if (configured_)
+  {
+    error = "ControllerRunner::configure() gọi lần thứ hai";
+    return false;
+  }
+
+  if (costmap == nullptr)
+  {
+    error = "ControllerRunner cần costmap local khác null";
+    return false;
+  }
+
+  nh_ = nh;
+  tf_ = tf;
+  costmap_ = costmap;
+  configured_ = true;
+
+  if (!initial_controller.empty() && !swapPlanner(initial_controller))
+  {
+    error = "không nạp được local planner khởi đầu '" + initial_controller + "'";
+    configured_ = false;
+    return false;
+  }
+
+  return true;
+}
+
+robot_nav_core::BaseLocalPlanner* ControllerRunner::acquire(const std::string& name)
+{
+  const auto cached = controllers_.find(name);
+  if (cached != controllers_.end())
+  {
+    return cached->second.instance.get();
+  }
+
+  robot::PluginLoaderHelper loader(nh_);
+  const std::string library_path = loader.findLibraryPath(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",
+                     name.c_str(), name.c_str());
+    return nullptr;
+  }
+
+  Loaded loaded;
+
+  try
+  {
+    loaded.factory = boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
+        library_path, name, boost::dll::load_mode::append_decorations);
+  }
+  catch (const boost::system::system_error& ex)
+  {
+    robot::log_error("[move_base2] ControllerRunner: không nạp được symbol '%s' từ '%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(),
+                     ex.what());
+    return nullptr;
+  }
+
+  try
+  {
+    loaded.instance = loaded.factory();
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] ControllerRunner: factory của '%s' ném 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());
+    return nullptr;
+  }
+
+  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_);
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] ControllerRunner: initialize() của '%s' ném exception: %s\n",
+                     name.c_str(), ex.what());
+    return nullptr;
+  }
+
+  const auto inserted = controllers_.emplace(name, std::move(loaded));
+  return inserted.first->second.instance.get();
+}
+
+void ControllerRunner::applyPendingLimits(robot_nav_core::BaseLocalPlanner* controller)
+{
+  if (controller == nullptr)
+  {
+    return;
+  }
+
+  // Trần vận tốc thuộc về YÊU CẦU, không thuộc về instance planner. Instance mới nạp không biết gì
+  // về các trần host đã đặt trước đó — không áp lại là robot lặng lẽ chạy nhanh hơn mức tầng an
+  // toàn cho phép, và không có dấu hiệu nào cả.
+  try
+  {
+    if (has_limit_linear_forward_)
+    {
+      controller->setTwistLinear(limit_linear_forward_);
+    }
+    if (has_limit_linear_backward_)
+    {
+      controller->setTwistLinear(limit_linear_backward_);
+    }
+    if (has_limit_angular_)
+    {
+      controller->setTwistAngular(limit_angular_);
+    }
+  }
+  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());
+  }
+}
+
+bool ControllerRunner::swapPlanner(const std::string& planner_name)
+{
+  if (!configured_)
+  {
+    robot::log_error("[move_base2] ControllerRunner: swapPlanner() trước configure().\n");
+    return false;
+  }
+
+  if (planner_name.empty())
+  {
+    robot::log_error("[move_base2] ControllerRunner: tên controller rỗng.\n");
+    return false;
+  }
+
+  if (planner_name == active_name_ && active_ != nullptr)
+  {
+    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);
+  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ề.
+    return false;
+  }
+
+  active_ = controller;
+  active_name_ = planner_name;
+  applyPendingLimits(active_);
+
+  robot::log_info("[move_base2] ControllerRunner: local planner đang dùng là '%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");
+    return false;
+  }
+
+  if (plan.empty())
+  {
+    // 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");
+    return false;
+  }
+
+  try
+  {
+    return active_->setPlan(plan);
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error_throttle(kHotPathLogThrottle,
+                              "[move_base2] ControllerRunner: '%s' ném exception trong setPlan: "
+                              "%s\n", active_name_.c_str(), ex.what());
+    return false;
+  }
+}
+
+bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
+{
+  cmd = robot_geometry_msgs::Twist();
+
+  if (!configured_ || active_ == nullptr)
+  {
+    robot::log_error_throttle(kHotPathLogThrottle,
+                              "[move_base2] ControllerRunner: computeVelocityCommands() khi chưa "
+                              "có controller.\n");
+    return false;
+  }
+
+  robot_geometry_msgs::Twist result;
+  bool ok = false;
+
+  try
+  {
+    ok = active_->computeVelocityCommands(measured_velocity_, result);
+  }
+  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)
+  {
+    return false;
+  }
+
+  if (!isFiniteTwist(result))
+  {
+    // 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",
+                              active_name_.c_str());
+    return false;
+  }
+
+  cmd = result;
+  return true;
+}
+
+bool ControllerRunner::isGoalReached()
+{
+  if (!configured_ || active_ == nullptr)
+  {
+    return false;
+  }
+
+  try
+  {
+    return active_->isGoalReached();
+  }
+  catch (const std::exception& ex)
+  {
+    // 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 "
+                              "isGoalReached: %s\n", active_name_.c_str(), ex.what());
+    return false;
+  }
+}
+
+void ControllerRunner::setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity)
+{
+  if (!isFiniteTwist(velocity))
+  {
+    // 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");
+    return;
+  }
+  measured_velocity_ = velocity;
+}
+
+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");
+    return false;
+  }
+
+  // Dấu chọn chiều — xem doc của ControllerPort::setTwistLinear. Nhớ cả hai chiều riêng để
+  // swapPlanner còn áp lại được lên instance mới.
+  if (linear.x < 0.0)
+  {
+    limit_linear_backward_ = linear;
+    has_limit_linear_backward_ = true;
+  }
+  else
+  {
+    limit_linear_forward_ = linear;
+    has_limit_linear_forward_ = true;
+  }
+
+  if (active_ == nullptr)
+  {
+    // Host đặt trần trước khi controller được nạp là chuyện bình thường: thứ tự khởi tạo không do
+    // move_base2 quyết. Đã nhớ lại, sẽ áp khi có controller.
+    return true;
+  }
+
+  try
+  {
+    return active_->setTwistLinear(linear);
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] ControllerRunner: '%s' ném exception trong setTwistLinear: %s\n",
+                     active_name_.c_str(), ex.what());
+    return false;
+  }
+}
+
+bool ControllerRunner::setTwistAngular(const robot_geometry_msgs::Vector3& angular)
+{
+  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");
+    return false;
+  }
+
+  limit_angular_ = angular;
+  has_limit_angular_ = true;
+
+  if (active_ == nullptr)
+  {
+    return true;
+  }
+
+  try
+  {
+    return active_->setTwistAngular(angular);
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] ControllerRunner: '%s' ném exception trong setTwistAngular: %s\n",
+                     active_name_.c_str(), ex.what());
+    return false;
+  }
+}
+
+std::string ControllerRunner::activeController() const
+{
+  return active_ != nullptr ? active_name_ : std::string();
+}
+
+}  // namespace move_base2