664 lines
21 KiB
C++
664 lines
21 KiB
C++
/*********************************************************************
|
|
*
|
|
* 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 <sstream>
|
|
#include <utility>
|
|
|
|
#include <boost/dll/import.hpp>
|
|
#include <boost/system/system_error.hpp>
|
|
|
|
#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
|
|
{
|
|
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;
|
|
|
|
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,
|
|
const std::string& initial_controller, std::string& error)
|
|
{
|
|
if (configured_)
|
|
{
|
|
error = "ControllerRunner::configure() called twice";
|
|
return false;
|
|
}
|
|
|
|
if (costmap == nullptr)
|
|
{
|
|
error = "ControllerRunner needs a non-null local costmap";
|
|
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 needs a non-null PosePort";
|
|
return false;
|
|
}
|
|
|
|
nh_ = nh;
|
|
tf_ = tf;
|
|
costmap_ = costmap;
|
|
pose_ = pose;
|
|
configured_ = true;
|
|
|
|
if (!initial_controller.empty() && !swapPlanner(initial_controller))
|
|
{
|
|
error = "could not load the initial local planner '" + initial_controller + "'";
|
|
configured_ = false;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
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();
|
|
}
|
|
|
|
robot::PluginLoaderHelper loader(nh_);
|
|
const std::string library_path = loader.findLibraryPath(name);
|
|
|
|
if (library_path.empty())
|
|
{
|
|
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;
|
|
}
|
|
|
|
Loaded loaded;
|
|
|
|
try
|
|
{
|
|
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)
|
|
{
|
|
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: error while loading '%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 of '%s' threw an exception: %s\n",
|
|
name.c_str(), ex.what());
|
|
return nullptr;
|
|
}
|
|
|
|
if (!loaded.instance)
|
|
{
|
|
robot::log_error("[move_base2] ControllerRunner: factory of '%s' returned nullptr.\n",
|
|
name.c_str());
|
|
return nullptr;
|
|
}
|
|
|
|
try
|
|
{
|
|
// 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)
|
|
{
|
|
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)
|
|
{
|
|
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: error while re-applying the velocity limits: "
|
|
"%s\n", ex.what());
|
|
}
|
|
}
|
|
|
|
bool ControllerRunner::swapPlanner(const std::string& planner_name)
|
|
{
|
|
if (!configured_)
|
|
{
|
|
robot::log_error("[move_base2] ControllerRunner: swapPlanner() before configure().\n");
|
|
return false;
|
|
}
|
|
|
|
if (planner_name.empty())
|
|
{
|
|
robot::log_error("[move_base2] ControllerRunner: empty controller name.\n");
|
|
return false;
|
|
}
|
|
|
|
if (planner_name == active_name_ && active_ != nullptr && !marker_dirty_)
|
|
{
|
|
// Đã đú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);
|
|
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;
|
|
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: active local planner is '%s'.\n",
|
|
planner_name.c_str());
|
|
return true;
|
|
}
|
|
|
|
bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan)
|
|
{
|
|
if (!configured_ || active_ == nullptr)
|
|
{
|
|
robot::log_error_throttle(kHotPathLogThrottle,
|
|
"[move_base2] ControllerRunner: setPlan() with no controller "
|
|
"loaded.\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: rejecting an empty plan.\n");
|
|
return false;
|
|
}
|
|
|
|
try
|
|
{
|
|
// 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 converted to Path2D came out "
|
|
"empty.\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;
|
|
active_plan_ = plan;
|
|
return true;
|
|
}
|
|
catch (const std::exception& ex)
|
|
{
|
|
robot::log_error_throttle(kHotPathLogThrottle,
|
|
"[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)
|
|
{
|
|
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();
|
|
|
|
if (!configured_ || active_ == nullptr)
|
|
{
|
|
robot::log_error_throttle(kHotPathLogThrottle,
|
|
"[move_base2] ControllerRunner: computeVelocityCommands() with no "
|
|
"controller loaded.\n");
|
|
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: pose lost, not computing a "
|
|
"command.\n");
|
|
return false;
|
|
}
|
|
|
|
robot_geometry_msgs::Twist result;
|
|
|
|
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 =
|
|
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' produced no command: %s\n",
|
|
active_name_.c_str(), ex.what());
|
|
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' returned a command containing "
|
|
"NaN/Inf.\n",
|
|
active_name_.c_str());
|
|
return false;
|
|
}
|
|
|
|
cmd = result;
|
|
return true;
|
|
}
|
|
|
|
bool ControllerRunner::isGoalReached()
|
|
{
|
|
if (!configured_ || active_ == nullptr)
|
|
{
|
|
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
|
|
{
|
|
const bool reached =
|
|
active_->isGoalReached(pose, robot_nav_2d_utils::twist3Dto2D(measured_velocity_));
|
|
if (reached)
|
|
{
|
|
has_active_goal_ = false;
|
|
active_plan_.clear();
|
|
}
|
|
return reached;
|
|
}
|
|
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' threw an exception in "
|
|
"isGoalReached: %s\n", active_name_.c_str(), ex.what());
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void ControllerRunner::getLocalPlan(robot_nav_2d_msgs::Path2D& plan)
|
|
{
|
|
plan = robot_nav_2d_msgs::Path2D();
|
|
|
|
if (!configured_ || active_ == nullptr)
|
|
{
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
ScopedSection timer(stats_, section_local_plan_);
|
|
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' could not return a local "
|
|
"trajectory: %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))
|
|
{
|
|
// 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: dropping a measured velocity "
|
|
"containing 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: linear velocity limit contains NaN/Inf, "
|
|
"ignored.\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' threw an exception in 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: angular velocity limit contains NaN/Inf, "
|
|
"ignored.\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' threw an exception in 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
|