DuongTD/recovery_core
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:32:40 +07:00
parent 89add78c7f
commit 887bff1b97
98 changed files with 8971 additions and 1339 deletions
Download Patch File Download Diff File Expand all files Collapse all files

238
plugins/back_up_recovery.cpp
View File

@@ -2,12 +2,13 @@
*
* Software License Agreement (BSD License)
*
* recovery_core — per-cycle backup recovery plugin (goal-driven).
* recovery_core — lùi thẳng một quãng, đo bằng pose thật và có kiểm va chạm.
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_math.h>
#include <algorithm>
#include <cmath>
@@ -20,18 +21,30 @@ namespace recovery_plugins
{
namespace
{
constexpr double kDefaultBackupDistance = 0.5; // m.
constexpr double kDefaultLinearSpeed = 0.1; // m/s.
constexpr double kDefaultControlPeriod = 0.1; // s per update tick.
constexpr double kDefaultBackupDistance = 0.3; // [m]
constexpr double kDefaultBackupDistanceMax = 1.0; // [m] trần cứng cho quãng lùi
constexpr double kDefaultLinearSpeed = 0.1; // [m/s] độ lớn
constexpr double kDefaultAccLimX = 0.3; // [m/s^2]
constexpr double kMaxLinearSpeed = 1.0; // [m/s] trần vệ sinh cho param sai
constexpr double kGoalTolerance = 1e-3; // [m]
} // namespace
/**
* @class BackUpRecovery
* @brief Lùi thẳng tới KHOẢNG ĐÍCH do caller yêu cầu ở start(goal).
* @brief Lùi thẳng theo hướng ban đầu tới khi đủ quãng yêu cầu.
*
* goal.distance (m, > 0) là khoảng lùi lượt này; 0 nghĩa là dùng default configured. Tốc độ
* tuyến tính mặc định đọc từ param, có thể override qua goal.params["linear_speed"]. Mỗi
* update() trả Twist.linear.x < 0 kèm progress/remaining tới khi đủ khoảng -> kSucceeded.
* Đây là behavior **nguy hiểm nhất** trong bộ default: lùi là hướng robot thường không có sensor.
* Vì vậy nó xếp cuối danh sách, và có ba lớp bảo vệ:
*
* 1. **Tiến độ đo bằng pose thật** — hình chiếu delta pose lên hướng xuất phát, không tích phân
* vận tốc lệnh. Bản trước nhân vận tốc lệnh với `control_period` lấy từ config, nên control
* loop chạy chậm gấp đôi là robot lùi gấp đôi quãng yêu cầu, còn bánh trượt thì vẫn báo xong.
* 2. **Kiểm va chạm mỗi tick** trên pose dự đoán ở cuối chu kỳ tới, trước khi phát lệnh; và một
* lần nữa lúc `onStart()` để không bao giờ khởi động vào chỗ đã bị chặn.
* 3. **Mất pose là dừng** — `PoseProvider` trả false thì trả `kFailed` + Twist 0.
*
* Quãng lùi: `goal.distance` cho lượt này, ngược lại param `backup_distance`; cả hai bị kẹp bởi
* `backup_distance_max`.
*/
class BackUpRecovery final : public recovery_core::RecoveryBehavior
{
@@ -43,101 +56,188 @@ public:
return std::make_shared<BackUpRecovery>();
}
recovery_core::RecoveryOutputType outputKind() const override
{
return recovery_core::RecoveryOutputType::kVelocity;
}
protected:
void onConfigure() override
bool onConfigure(robot::NodeHandle& nh) override
{
robot::NodeHandle private_nh("~/" + name_);
private_nh.param("backup_distance", default_backup_distance_, kDefaultBackupDistance);
private_nh.param("linear_speed", default_linear_speed_, kDefaultLinearSpeed);
private_nh.param("control_period", control_period_, kDefaultControlPeriod);
private_nh.param("require_costmap", require_costmap_, false);
nh.param("backup_distance", default_backup_distance_, kDefaultBackupDistance);
nh.param("backup_distance_max", backup_distance_max_, kDefaultBackupDistanceMax);
nh.param("linear_speed", default_linear_speed_, kDefaultLinearSpeed);
nh.param("acc_lim_x", acc_lim_x_, kDefaultAccLimX);
if (!std::isfinite(default_backup_distance_) || default_backup_distance_ <= 0.0)
if (!std::isfinite(backup_distance_max_) || backup_distance_max_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid backup_distance for '%s'; using 0.5 m.",
name_.c_str());
default_backup_distance_ = kDefaultBackupDistance;
robot::log_warning("[recovery_core] '%s': backup_distance_max=%.3f m is invalid; using %.3f "
"m.", name().c_str(), backup_distance_max_,
kDefaultBackupDistanceMax);
backup_distance_max_ = kDefaultBackupDistanceMax;
}
if (!std::isfinite(default_linear_speed_) || default_linear_speed_ <= 0.0)
default_backup_distance_ = clampDistance(default_backup_distance_, kDefaultBackupDistance);
default_linear_speed_ = clampSpeed(default_linear_speed_, kDefaultLinearSpeed);
if (!std::isfinite(acc_lim_x_) || acc_lim_x_ < 0.0)
{
robot::log_warning("[recovery_core] Invalid linear_speed for '%s'; using 0.1 m/s.",
name_.c_str());
default_linear_speed_ = kDefaultLinearSpeed;
}
if (!std::isfinite(control_period_) || control_period_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid control_period for '%s'; using 0.1 s.",
name_.c_str());
control_period_ = kDefaultControlPeriod;
robot::log_warning("[recovery_core] '%s': acc_lim_x=%.3f m/s^2 is invalid; using %.3f.",
name().c_str(), acc_lim_x_, kDefaultAccLimX);
acc_lim_x_ = kDefaultAccLimX;
}
return true;
}
recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override
bool onStart(const recovery_core::RecoveryGoal& goal) override
{
// Costmap là bắt buộc? fail sớm trước khi xuất vận tốc lùi.
if (require_costmap_ && ctx().local_costmap == nullptr)
// Base đã bảo đảm ctx().pose và ctx().collision khác null cho họ velocity.
if (!ctx().pose->getRobotPose(start_pose_))
{
return recovery_core::RecoveryResult::Failed().withMessage("backup requires local costmap");
robot::log_warning("[recovery_core] '%s': could not get a pose at start-up (TF stale?).",
name().c_str());
return false;
}
backup_distance_ = (std::isfinite(goal.distance) && goal.distance > 0.0)
? goal.distance
: default_backup_distance_;
start_yaw_ = recovery_core::yawOf(start_pose_);
linear_speed_ = std::abs(goal.param("linear_speed", default_linear_speed_));
if (!std::isfinite(linear_speed_) || linear_speed_ <= 0.0)
backup_distance_ = clampDistance(goal.distance.value_or(default_backup_distance_),
default_backup_distance_);
linear_speed_ = clampSpeed(std::abs(goal.param("linear_speed", default_linear_speed_)),
default_linear_speed_);
current_speed_ = 0.0;
// Kiểm ngay tại chỗ: nếu vị trí lùi đầu tiên đã bị chặn thì từ chối khởi động, để state machine
// chuyển sang behavior kế tiếp thay vì phát một lệnh lùi rồi mới hỏng.
if (blockedAhead(start_pose_, kProbeDistance))
{
linear_speed_ = default_linear_speed_;
robot::log_warning("[recovery_core] '%s': the space behind is already blocked, refusing to "
"back up.",
name().c_str());
return false;
}
traveled_distance_ = 0.0;
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
recovery_core::RecoveryStatus::kRunning)
.withProgress(0.0, backup_distance_)
.withMessage("backup start");
return true;
}
recovery_core::RecoveryResult onUpdate() override
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double dt) override
{
if (traveled_distance_ >= backup_distance_)
robot_geometry_msgs::PoseStamped pose;
if (!ctx().pose->getRobotPose(pose))
{
return succeeded();
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kFailed)
.withMessage("robot pose lost (TF stale?) — stopping the back-up");
}
// Tiến độ = phần đi ngược hướng xuất phát. Dấu âm của hình chiếu chính là quãng đã lùi.
const double traveled = -recovery_core::projectOntoHeading(pose, start_pose_, start_yaw_);
const double remaining = backup_distance_ - traveled;
if (remaining <= kGoalTolerance)
{
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kSucceeded)
.withProgress(1.0, 0.0)
.withMessage("backup complete");
}
// Vận tốc của tick này, đã ramp theo trần gia tốc, và không vượt quãng còn lại nếu dt cho phép.
double speed = recovery_core::rampToward(linear_speed_, current_speed_, acc_lim_x_, dt);
if (dt > 0.0)
{
speed = std::min(speed, remaining / dt);
}
speed = std::max(speed, 0.0);
// Pose dự đoán ở CUỐI chu kỳ tới — kiểm trước khi phát lệnh, không phải sau.
const double probe = std::max(speed * std::max(dt, kMinProbeDt), kProbeDistance);
if (blockedAhead(pose, probe))
{
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kFailed)
.withMessage("obstacle behind — cancelling the back-up");
}
current_speed_ = speed;
robot_geometry_msgs::Twist command;
command.linear.x = -std::abs(linear_speed_);
traveled_distance_ = std::min(
backup_distance_, traveled_distance_ + std::abs(command.linear.x) * control_period_);
if (traveled_distance_ >= backup_distance_)
{
return succeeded();
}
command.linear.x = -speed; // [m/s], âm = lùi
return recovery_core::RecoveryResult::Velocity(command,
recovery_core::RecoveryStatus::kRunning)
.withProgress(traveled_distance_ / backup_distance_,
backup_distance_ - traveled_distance_)
.withProgress(traveled / backup_distance_, remaining)
.withMessage("backing up");
}
private:
recovery_core::RecoveryResult succeeded()
recovery_core::RecoveryResult onCancel() override
{
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
recovery_core::RecoveryStatus::kSucceeded)
.withProgress(1.0, 0.0)
.withMessage("backup complete");
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kCancelled).withMessage("backup cancelled");
}
double default_backup_distance_ = kDefaultBackupDistance;
double default_linear_speed_ = kDefaultLinearSpeed;
double control_period_ = kDefaultControlPeriod;
bool require_costmap_ = false;
private:
/// Khoảng dò tối thiểu [m] — luôn nhìn trước ít nhất một ô costmap kể cả khi dt rất nhỏ.
static constexpr double kProbeDistance = 0.05;
/// dt tối thiểu [s] dùng khi dự đoán, để tick đầu (dt = 0) vẫn dò về phía trước.
static constexpr double kMinProbeDt = 0.1;
double backup_distance_ = kDefaultBackupDistance;
double linear_speed_ = kDefaultLinearSpeed;
double traveled_distance_ = 0.0;
/// @return true nếu đặt robot lùi thêm @p distance từ @p from là va chạm.
bool blockedAhead(const robot_geometry_msgs::PoseStamped& from, double distance) const
{
const double next_x = from.pose.position.x - std::cos(start_yaw_) * distance;
const double next_y = from.pose.position.y - std::sin(start_yaw_) * distance;
return ctx().collision->footprintCost(next_x, next_y, start_yaw_) < 0.0;
}
double clampDistance(double value, double fallback) const
{
if (!std::isfinite(value) || value <= 0.0)
{
robot::log_warning("[recovery_core] '%s': backup_distance=%.3f m is invalid; using %.3f m.",
name().c_str(), value, fallback);
return std::min(fallback, backup_distance_max_);
}
if (value > backup_distance_max_)
{
robot::log_warning("[recovery_core] '%s': backup_distance=%.3f m > limit %.3f m; clamped.",
name().c_str(), value, backup_distance_max_);
return backup_distance_max_;
}
return value;
}
double clampSpeed(double value, double fallback) const
{
if (!std::isfinite(value) || value <= 0.0)
{
robot::log_warning("[recovery_core] '%s': linear_speed=%.3f m/s is invalid; using %.3f m/s.",
name().c_str(), value, fallback);
return fallback;
}
if (value > kMaxLinearSpeed)
{
robot::log_warning("[recovery_core] '%s': linear_speed=%.3f m/s > limit %.3f m/s; clamped.",
name().c_str(), value, kMaxLinearSpeed);
return kMaxLinearSpeed;
}
return value;
}
// Config
double default_backup_distance_ = kDefaultBackupDistance; ///< [m]
double backup_distance_max_ = kDefaultBackupDistanceMax; ///< [m]
double default_linear_speed_ = kDefaultLinearSpeed; ///< [m/s]
double acc_lim_x_ = kDefaultAccLimX; ///< [m/s^2]
// Trạng thái lượt hiện tại
robot_geometry_msgs::PoseStamped start_pose_;
double start_yaw_ = 0.0; ///< [rad]
double backup_distance_ = kDefaultBackupDistance; ///< [m]
double linear_speed_ = kDefaultLinearSpeed; ///< [m/s]
double current_speed_ = 0.0; ///< [m/s] đang phát, để ramp
};
} // namespace recovery_plugins

182
plugins/clear_costmap_recovery.cpp
View File

@@ -2,7 +2,7 @@
*
* Software License Agreement (BSD License)
*
* recovery_core — no-output clear costmap plugin.
* recovery_core — xoá vật cản đã tích trong costmap. Một tick, không output.
*
* Author: DuongTD
*********************************************************************/
@@ -19,12 +19,16 @@
#include <boost/pointer_cast.hpp>
#include <boost/thread/locks.hpp>
#include <robot/robot.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_layer.h>
namespace recovery_plugins
{
namespace
{
constexpr double kDefaultResetDistance = 3.0; // [m]
constexpr double kMaxResetDistance = 100.0; // [m] trần vệ sinh cho param sai
std::string leafName(std::string name)
{
const std::string::size_type slash = name.rfind('/');
@@ -34,13 +38,23 @@ std::string leafName(std::string name)
}
return name;
}
bool isValidResetDistance(double value)
{
return std::isfinite(value) && value > 0.0;
}
} // namespace
/**
* @class ClearCostmapRecovery
* @brief Xoá vùng vật cản đã tích trong các layer được chỉ định.
*
* Dùng **hai instance** trong bộ default:
* - `conservative_reset` (`invert_area_to_clear: false`) xoá vùng gần robot;
* - `aggressive_reset` (`invert_area_to_clear: true`) xoá mọi thứ **ngoài** vùng đó.
*
* Không phát output, hoàn tất trong một tick.
*
* @note Cố ý **không** gọi `Costmap2DROBOT::updateMap()`. Hàm đó giữ mutex master rồi chạy toàn bộ
* chuỗi layer `updateBounds`/`updateCosts` — cỡ chục tới trăm ms — trong khi behavior này
* chạy trên thread phát cmd_vel ở 30 Hz. Costmap tự update ở chu kỳ riêng của nó ngay sau
* đó; ép update tại đây chỉ để đổi lấy một chu kỳ control bị lỡ.
*/
class ClearCostmapRecovery final : public recovery_core::RecoveryBehavior
{
public:
@@ -51,82 +65,107 @@ public:
return std::make_shared<ClearCostmapRecovery>();
}
protected:
void onConfigure() override
recovery_core::RecoveryOutputType outputKind() const override
{
robot::NodeHandle private_nh("~/" + name_);
private_nh.param("reset_distance", reset_distance_, 3.0);
private_nh.param("invert_area_to_clear", invert_area_to_clear_, false);
private_nh.param("force_updating", force_updating_, false);
private_nh.param("affected_maps", affected_maps_, std::string("both"));
return recovery_core::RecoveryOutputType::kNone;
}
if (!isValidResetDistance(reset_distance_))
protected:
bool onConfigure(robot::NodeHandle& nh) override
{
nh.param("reset_distance", reset_distance_, kDefaultResetDistance);
nh.param("invert_area_to_clear", invert_area_to_clear_, false);
nh.param("affected_maps", affected_maps_, std::string("both"));
if (!std::isfinite(reset_distance_) || reset_distance_ <= 0.0 ||
reset_distance_ > kMaxResetDistance)
{
robot::log_warning("[recovery_core] Invalid reset_distance for '%s'; using 3.0 m.",
name_.c_str());
reset_distance_ = 3.0;
robot::log_warning("[recovery_core] '%s': reset_distance=%.3f m outside (0, %.0f]; using "
"%.3f m.", name().c_str(), reset_distance_, kMaxResetDistance,
kDefaultResetDistance);
reset_distance_ = kDefaultResetDistance;
}
if (affected_maps_ != "local" && affected_maps_ != "global" && affected_maps_ != "both")
{
robot::log_warning("[recovery_core] Invalid affected_maps '%s' for '%s'; using 'both'.",
affected_maps_.c_str(), name_.c_str());
robot::log_warning("[recovery_core] '%s': affected_maps='%s' is invalid; using 'both'.",
name().c_str(), affected_maps_.c_str());
affected_maps_ = "both";
}
std::vector<std::string> clearable_layers_default;
clearable_layers_default.emplace_back("obstacles");
std::vector<std::string> clearable_layers_default{"obstacles"};
std::vector<std::string> clearable_layers;
private_nh.param("layer_names", clearable_layers, clearable_layers_default);
nh.param("layer_names", clearable_layers, clearable_layers_default);
clearable_layers_.insert(clearable_layers.begin(), clearable_layers.end());
if (clearable_layers_.empty())
{
robot::log_error("[recovery_core] '%s': layer_names is empty — this behavior will not clear "
"anything.",
name().c_str());
return false;
}
const bool needs_global = affected_maps_ == "global" || affected_maps_ == "both";
const bool needs_local = affected_maps_ == "local" || affected_maps_ == "both";
if (needs_global && ctx().global_costmap == nullptr)
{
robot::log_error("[recovery_core] '%s': affected_maps='%s' but the global costmap is "
"missing.",
name().c_str(), affected_maps_.c_str());
return false;
}
if (needs_local && ctx().local_costmap == nullptr)
{
robot::log_error("[recovery_core] '%s': affected_maps='%s' but the local costmap is missing.",
name().c_str(), affected_maps_.c_str());
return false;
}
return true;
}
recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& /*goal*/) override
bool onStart(const recovery_core::RecoveryGoal& /*goal*/) override
{
// One-shot: công việc thực hiện ở onUpdate() lần đầu.
return recovery_core::RecoveryResult::Running().withMessage("clear costmap start");
// One-shot: công việc thực hiện ở tick đầu tiên, giữ start() không có tác dụng phụ.
return true;
}
recovery_core::RecoveryResult onUpdate() override
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double /*dt*/) override
{
bool ok = true;
if (affected_maps_ == "global" || affected_maps_ == "both")
{
ok = clear(ctx().global_costmap) && ok;
if (ok && force_updating_ && ctx().global_costmap != nullptr)
{
ctx().global_costmap->updateMap();
}
ok = clear(ctx().global_costmap, "global") && ok;
}
if (affected_maps_ == "local" || affected_maps_ == "both")
{
ok = clear(ctx().local_costmap) && ok;
if (ok && force_updating_ && ctx().local_costmap != nullptr)
{
ctx().local_costmap->updateMap();
}
ok = clear(ctx().local_costmap, "local") && ok;
}
return ok ? recovery_core::RecoveryResult::Succeeded().withMessage("clear costmap complete")
return ok ? recovery_core::RecoveryResult::Succeeded()
.withProgress(1.0, 0.0)
.withMessage("clear costmap complete")
: recovery_core::RecoveryResult::Failed().withMessage("clear costmap failed");
}
private:
bool clear(robot_costmap_2d::Costmap2DROBOT* costmap)
bool clear(robot_costmap_2d::Costmap2DROBOT* costmap, const char* which)
{
if (costmap == nullptr || costmap->getLayeredCostmap() == nullptr)
{
robot::log_error("[recovery_core] ClearCostmapRecovery '%s' missing costmap.",
name_.c_str());
robot::log_error("[recovery_core] '%s': %s costmap is missing.", name().c_str(), which);
return false;
}
robot_geometry_msgs::PoseStamped pose;
if (!costmap->getRobotPose(pose))
{
robot::log_error("[recovery_core] ClearCostmapRecovery '%s' cannot get robot pose.",
name_.c_str());
robot::log_error("[recovery_core] '%s': could not get the robot pose on the %s costmap.",
name().c_str(), which);
return false;
}
@@ -134,12 +173,14 @@ private:
costmap->getLayeredCostmap()->getPlugins();
if (plugins == nullptr)
{
robot::log_error("[recovery_core] ClearCostmapRecovery '%s' missing costmap layers.",
name_.c_str());
robot::log_error("[recovery_core] '%s': %s costmap has no layer.", name().c_str(),
which);
return false;
}
bool touched_layer = false;
std::string available;
for (const boost::shared_ptr<robot_costmap_2d::Layer>& plugin : *plugins)
{
if (!plugin)
@@ -147,16 +188,23 @@ private:
continue;
}
const std::string name = leafName(plugin->getName());
if (clearable_layers_.count(name) == 0)
const std::string layer_name = leafName(plugin->getName());
if (!available.empty())
{
available += ", ";
}
available += layer_name;
if (clearable_layers_.count(layer_name) == 0)
{
continue;
}
if (dynamic_cast<robot_costmap_2d::CostmapLayer*>(plugin.get()) == nullptr)
{
robot::log_warning("[recovery_core] Layer '%s' is not a CostmapLayer; skipped.",
name.c_str());
robot::log_warning("[recovery_core] '%s': layer '%s' is not a CostmapLayer; skipped.",
name().c_str(), layer_name.c_str());
continue;
}
@@ -165,13 +213,22 @@ private:
touched_layer = true;
}
return touched_layer;
if (!touched_layer)
{
// Nguyên nhân phổ biến nhất của "recovery này không làm gì" là sai tên layer trong config.
// Bản trước trả kFailed lặng lẽ, nên không có cách nào biết vì sao.
robot::log_error("[recovery_core] '%s': no layer of the %s costmap matches layer_names. "
"Layers present: [%s].", name().c_str(), which, available.c_str());
return false;
}
return true;
}
void clearMap(const boost::shared_ptr<robot_costmap_2d::CostmapLayer>& costmap,
double pose_x, double pose_y)
void clearMap(const boost::shared_ptr<robot_costmap_2d::CostmapLayer>& layer, double pose_x,
double pose_y)
{
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(layer->getMutex()));
const double start_point_x = pose_x - reset_distance_ / 2.0;
const double start_point_y = pose_y - reset_distance_ / 2.0;
@@ -182,19 +239,20 @@ private:
int start_y = 0;
int end_x = 0;
int end_y = 0;
costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
layer->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
layer->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
costmap->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);
costmap->addExtraBounds(costmap->getOriginX(), costmap->getOriginY(),
costmap->getOriginX() + costmap->getSizeInMetersX(),
costmap->getOriginY() + costmap->getSizeInMetersY());
layer->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);
// Báo cho layer biết toàn bộ vùng của nó cần được ghi lại vào master ở chu kỳ update kế tiếp.
layer->addExtraBounds(layer->getOriginX(), layer->getOriginY(),
layer->getOriginX() + layer->getSizeInMetersX(),
layer->getOriginY() + layer->getSizeInMetersY());
}
bool force_updating_ = false;
double reset_distance_ = 3.0;
bool invert_area_to_clear_ = false;
std::string affected_maps_ = "both";
double reset_distance_ = kDefaultResetDistance; ///< [m] cạnh vùng vuông quanh robot
bool invert_area_to_clear_ = false; ///< true = xoá phần NGOÀI vùng
std::string affected_maps_ = "both"; ///< local | global | both
std::set<std::string> clearable_layers_;
};

62
plugins/regen_path_recovery.cpp
View File

@@ -1,62 +0,0 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* recovery_core — path output recovery plugin (goal-driven, one-shot).
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_behavior.h>
#include <boost/dll/alias.hpp>
#include <robot/robot.h>
namespace recovery_plugins
{
/**
* @class RegenPathRecovery
* @brief Họ A (path output), one-shot: trả lại robot_nav_msgs::Path từ global_path hiện tại.
*
* Hoàn tất ngay ở lần update() đầu. Guard chưa configure/global_path null hoặc rỗng -> Failed().
*/
class RegenPathRecovery final : public recovery_core::RecoveryBehavior
{
public:
RegenPathRecovery() = default;
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
{
return std::make_shared<RegenPathRecovery>();
}
protected:
recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& /*goal*/) override
{
// One-shot: công việc thực hiện ở onUpdate() lần đầu, giữ start() gọn.
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
recovery_core::RecoveryStatus::kRunning)
.withMessage("regen path start");
}
recovery_core::RecoveryResult onUpdate() override
{
const auto* global_path = ctx().global_path;
if (global_path == nullptr || global_path->empty())
{
return recovery_core::RecoveryResult::Failed().withMessage("no global path to regenerate");
}
robot_nav_msgs::Path path;
path.poses = *global_path;
return recovery_core::RecoveryResult::PathOut(path, recovery_core::RecoveryStatus::kSucceeded)
.withProgress(1.0, 0.0)
.withMessage("regen path complete");
}
};
} // namespace recovery_plugins
BOOST_DLL_ALIAS(recovery_plugins::RegenPathRecovery::create, RegenPathRecovery)

269
plugins/rotate_recovery.cpp
View File

@@ -2,12 +2,13 @@
*
* Software License Agreement (BSD License)
*
* recovery_core — per-cycle rotate recovery plugin (goal-driven).
* recovery_core — quay tại chỗ, quét cung trước khi quay và đo bằng pose thật.
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_math.h>
#include <algorithm>
#include <cmath>
@@ -20,18 +21,30 @@ namespace recovery_plugins
{
namespace
{
constexpr double kDefaultTargetAngle = 1.57079632679; // pi/2 rad.
constexpr double kDefaultAngularSpeed = 0.4; // rad/s.
constexpr double kDefaultControlPeriod = 0.1; // s per update tick.
constexpr double kTwoPi = 2.0 * M_PI;
constexpr double kDefaultAngularSpeed = 0.4; // [rad/s] độ lớn
constexpr double kDefaultAccLimTheta = 0.8; // [rad/s^2]
constexpr double kDefaultSimGranularity = 0.1; // [rad] bước quét cung
constexpr double kMaxAngularSpeed = 2.0; // [rad/s] trần vệ sinh cho param sai
constexpr double kGoalTolerance = 1e-3; // [rad]
} // namespace
/**
* @class RotateRecovery
* @brief Quay tại chỗ tới GÓC ĐÍCH do caller yêu cầu ở start(goal).
* @brief Quay tại chỗ, mặc định đủ một vòng, để costmap quan sát lại xung quanh.
*
* goal.angle (rad, có dấu) là góc quay lượt này; 0 nghĩa là dùng default configured. Tốc độ
* góc mặc định đọc từ param, có thể override qua goal.params["angular_speed"]. Mỗi update() trả
* Twist.angular.z kèm progress/remaining tới khi đủ góc -> kSucceeded (zero command).
* Quay đủ 2π là công dụng chính của rotate trong một bộ recovery: nó cho obstacle/voxel layer nhìn
* thấy toàn bộ vùng quanh robot rồi mới lập plan lại. Đặt `full_rotation: false` hoặc truyền
* `goal.angle` để quay một góc cụ thể.
*
* Hai lớp bảo vệ so với bản trước (bản trước không dùng `ctx()` một lần nào):
* 1. **Quét toàn bộ cung sẽ quay** tại `onStart()` theo bước `sim_granularity`; chạm vật cản ở bất
* kỳ góc nào là từ chối khởi động, chứ không quay tới nơi mới phát hiện.
* 2. **Tiến độ đo bằng pose thật**, cộng dồn từng chênh lệch yaw đã chuẩn hoá — nên quay > π vẫn
* đếm đúng, và loop chạy chậm không làm robot quay quá góc.
*
* @note Yêu cầu robot xoay tại chỗ được (differential/omni). Config workspace hiện tại thoả:
* `min_turn_radius: 0.0`, `use_rotate_to_heading: true`.
*/
class RotateRecovery final : public recovery_core::RecoveryBehavior
{
@@ -43,95 +56,221 @@ public:
return std::make_shared<RotateRecovery>();
}
recovery_core::RecoveryOutputType outputKind() const override
{
return recovery_core::RecoveryOutputType::kVelocity;
}
protected:
void onConfigure() override
bool onConfigure(robot::NodeHandle& nh) override
{
robot::NodeHandle private_nh("~/" + name_);
private_nh.param("target_angle", default_target_angle_, kDefaultTargetAngle);
private_nh.param("angular_speed", default_angular_speed_, kDefaultAngularSpeed);
private_nh.param("control_period", control_period_, kDefaultControlPeriod);
nh.param("full_rotation", full_rotation_, true);
nh.param("target_angle", default_target_angle_, kTwoPi);
nh.param("angular_speed", default_angular_speed_, kDefaultAngularSpeed);
nh.param("acc_lim_theta", acc_lim_theta_, kDefaultAccLimTheta);
nh.param("sim_granularity", sim_granularity_, kDefaultSimGranularity);
if (!std::isfinite(default_target_angle_) || std::abs(default_target_angle_) <= 0.0)
default_target_angle_ = clampAngle(default_target_angle_, kTwoPi);
default_angular_speed_ = clampSpeed(default_angular_speed_, kDefaultAngularSpeed);
if (!std::isfinite(acc_lim_theta_) || acc_lim_theta_ < 0.0)
{
robot::log_warning("[recovery_core] Invalid target_angle for '%s'; using pi/2.",
name_.c_str());
default_target_angle_ = kDefaultTargetAngle;
robot::log_warning("[recovery_core] '%s': acc_lim_theta=%.3f rad/s^2 is invalid; using %.3f.",
name().c_str(), acc_lim_theta_, kDefaultAccLimTheta);
acc_lim_theta_ = kDefaultAccLimTheta;
}
if (!std::isfinite(default_angular_speed_) || default_angular_speed_ <= 0.0)
if (!std::isfinite(sim_granularity_) || sim_granularity_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid angular_speed for '%s'; using 0.4 rad/s.",
name_.c_str());
default_angular_speed_ = kDefaultAngularSpeed;
}
if (!std::isfinite(control_period_) || control_period_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid control_period for '%s'; using 0.1 s.",
name_.c_str());
control_period_ = kDefaultControlPeriod;
robot::log_warning("[recovery_core] '%s': sim_granularity=%.3f rad is invalid; using %.3f "
"rad.", name().c_str(), sim_granularity_, kDefaultSimGranularity);
sim_granularity_ = kDefaultSimGranularity;
}
return true;
}
recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override
bool onStart(const recovery_core::RecoveryGoal& goal) override
{
// Góc đích: goal.angle nếu hợp lệ, ngược lại default configured.
target_angle_ = (std::isfinite(goal.angle) && std::abs(goal.angle) > 0.0)
? goal.angle
: default_target_angle_;
angular_speed_ = std::abs(goal.param("angular_speed", default_angular_speed_));
if (!std::isfinite(angular_speed_) || angular_speed_ <= 0.0)
robot_geometry_msgs::PoseStamped pose;
if (!ctx().pose->getRobotPose(pose))
{
angular_speed_ = default_angular_speed_;
robot::log_warning("[recovery_core] '%s': could not get a pose at start-up (TF stale?).",
name().c_str());
return false;
}
rotated_angle_ = 0.0;
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
recovery_core::RecoveryStatus::kRunning)
.withProgress(0.0, std::abs(target_angle_))
.withMessage("rotate start");
start_pose_ = pose;
start_yaw_ = recovery_core::yawOf(pose);
last_yaw_ = start_yaw_;
rotated_ = 0.0;
current_speed_ = 0.0;
// goal.angle có giá trị (kể cả 0.0) thì tôn trọng đúng giá trị đó. Bản trước coi 0 là "chưa
// đặt" nên một góc tính ra ~0 bị âm thầm thay bằng pi/2.
const double requested = goal.angle.value_or(full_rotation_ ? kTwoPi : default_target_angle_);
target_angle_ = clampAngle(requested, default_target_angle_);
angular_speed_ = clampSpeed(std::abs(goal.param("angular_speed", default_angular_speed_)),
default_angular_speed_);
if (std::abs(target_angle_) <= kGoalTolerance)
{
// Caller nói rõ "đừng quay". Đó là một yêu cầu hợp lệ và hoàn tất ngay.
zero_rotation_ = true;
return true;
}
zero_rotation_ = false;
if (!arcIsClear())
{
robot::log_warning("[recovery_core] '%s': the %.3f rad arc is blocked, refusing to rotate.",
name().c_str(), target_angle_);
return false;
}
return true;
}
recovery_core::RecoveryResult onUpdate() override
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double dt) override
{
const double target = std::abs(target_angle_);
if (rotated_angle_ >= target)
if (zero_rotation_)
{
return succeeded(target);
return stopResult(recovery_core::RecoveryStatus::kSucceeded)
.withProgress(1.0, 0.0)
.withMessage("rotate 0 rad — nothing to rotate");
}
robot_geometry_msgs::PoseStamped pose;
if (!ctx().pose->getRobotPose(pose))
{
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kFailed)
.withMessage("robot pose lost (TF stale?) — stopping the rotation");
}
const double yaw = recovery_core::yawOf(pose);
// Cộng dồn từng bước đã chuẩn hoá: cách duy nhất đếm đúng khi tổng góc quay vượt pi.
rotated_ += std::abs(recovery_core::normalizeAngle(yaw - last_yaw_));
last_yaw_ = yaw;
const double remaining = target - rotated_;
if (remaining <= kGoalTolerance)
{
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kSucceeded)
.withProgress(1.0, 0.0)
.withMessage("rotate complete");
}
double speed = recovery_core::rampToward(angular_speed_, current_speed_, acc_lim_theta_, dt);
if (dt > 0.0)
{
speed = std::min(speed, remaining / dt);
}
speed = std::max(speed, 0.0);
// Cung đã quét ở onStart(), nhưng costmap đổi giữa chừng thì phải phát hiện: kiểm góc dự đoán
// ở cuối chu kỳ tới trước khi phát lệnh.
const double direction = target_angle_ >= 0.0 ? 1.0 : -1.0;
const double next_yaw = yaw + direction * std::max(speed * dt, sim_granularity_);
if (ctx().collision->footprintCost(pose.pose.position.x, pose.pose.position.y, next_yaw) < 0.0)
{
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kFailed)
.withMessage("the rotation arc became blocked midway — stopping the rotation");
}
current_speed_ = speed;
robot_geometry_msgs::Twist command;
command.angular.z = std::copysign(angular_speed_, target_angle_);
rotated_angle_ =
std::min(target, rotated_angle_ + std::abs(command.angular.z) * control_period_);
if (rotated_angle_ >= target)
{
return succeeded(target);
}
command.angular.z = std::copysign(speed, target_angle_); // [rad/s], + = ngược chiều kim đồng hồ
return recovery_core::RecoveryResult::Velocity(command,
recovery_core::RecoveryStatus::kRunning)
.withProgress(rotated_angle_ / target, target - rotated_angle_)
.withProgress(rotated_ / target, remaining)
.withMessage("rotating");
}
private:
recovery_core::RecoveryResult succeeded(double target)
recovery_core::RecoveryResult onCancel() override
{
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
recovery_core::RecoveryStatus::kSucceeded)
.withProgress(1.0, 0.0)
.withMessage("rotate complete");
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kCancelled).withMessage("rotate cancelled");
}
double default_target_angle_ = kDefaultTargetAngle;
double default_angular_speed_ = kDefaultAngularSpeed;
double control_period_ = kDefaultControlPeriod;
private:
/// @return true nếu toàn bộ cung sẽ quay đều đặt được footprint.
bool arcIsClear() const
{
const double target = std::abs(target_angle_);
const double direction = target_angle_ >= 0.0 ? 1.0 : -1.0;
const double x = start_pose_.pose.position.x;
const double y = start_pose_.pose.position.y;
double target_angle_ = kDefaultTargetAngle;
double angular_speed_ = kDefaultAngularSpeed;
double rotated_angle_ = 0.0;
for (double swept = 0.0; swept < target; swept += sim_granularity_)
{
if (ctx().collision->footprintCost(x, y, start_yaw_ + direction * swept) < 0.0)
{
return false;
}
}
// Kiểm luôn góc cuối: vòng lặp trên dừng trước target nếu target không chia hết cho bước quét.
return ctx().collision->footprintCost(x, y, start_yaw_ + direction * target) >= 0.0;
}
double clampAngle(double value, double fallback) const
{
if (!std::isfinite(value))
{
robot::log_warning("[recovery_core] '%s': target_angle is not finite; using %.3f rad.",
name().c_str(), fallback);
return fallback;
}
if (std::abs(value) > kTwoPi)
{
robot::log_warning("[recovery_core] '%s': target_angle=%.3f rad exceeds +/-2pi; clamped.",
name().c_str(), value);
return std::copysign(kTwoPi, value);
}
return value;
}
double clampSpeed(double value, double fallback) const
{
if (!std::isfinite(value) || value <= 0.0)
{
robot::log_warning("[recovery_core] '%s': angular_speed=%.3f rad/s is invalid; using %.3f "
"rad/s.", name().c_str(), value, fallback);
return fallback;
}
if (value > kMaxAngularSpeed)
{
robot::log_warning("[recovery_core] '%s': angular_speed=%.3f rad/s > limit %.3f; clamped.",
name().c_str(), value, kMaxAngularSpeed);
return kMaxAngularSpeed;
}
return value;
}
// Config
bool full_rotation_ = true;
double default_target_angle_ = kTwoPi; ///< [rad]
double default_angular_speed_ = kDefaultAngularSpeed; ///< [rad/s]
double acc_lim_theta_ = kDefaultAccLimTheta; ///< [rad/s^2]
double sim_granularity_ = kDefaultSimGranularity; ///< [rad]
// Trạng thái lượt hiện tại
robot_geometry_msgs::PoseStamped start_pose_;
double start_yaw_ = 0.0; ///< [rad]
double last_yaw_ = 0.0; ///< [rad]
double rotated_ = 0.0; ///< [rad] cộng dồn, luôn >= 0
double target_angle_ = kTwoPi; ///< [rad] có dấu
double angular_speed_ = kDefaultAngularSpeed; ///< [rad/s] độ lớn
double current_speed_ = 0.0; ///< [rad/s] đang phát, để ramp
bool zero_rotation_ = false; ///< caller yêu cầu góc 0
};
} // namespace recovery_plugins

103
plugins/wait_recovery.cpp Normal file
View File

@@ -0,0 +1,103 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* recovery_core — đợi tại chỗ, không phát output.
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_behavior.h>
#include <cmath>
#include <string>
#include <boost/dll/alias.hpp>
#include <robot/robot.h>
namespace recovery_plugins
{
namespace
{
constexpr double kDefaultWaitDuration = 3.0; // [s]
constexpr double kMaxWaitDuration = 300.0; // [s] trần vệ sinh cho param cấu hình sai.
} // namespace
/**
* @class WaitRecovery
* @brief Đứng yên một khoảng thời gian rồi báo thành công.
*
* Đây là recovery **an toàn nhất** trong bộ default và nên đứng đầu danh sách: nó không di chuyển,
* không cần pose, không cần collision check. Với AMR/AGV trong kho, phần lớn tình huống chặn đường
* là vật cản động (người, xe khác) — đợi vài giây rồi lập plan lại giải quyết được đa số, trong khi
* mọi behavior khác đều bắt robot cử động trong lúc chưa biết chuyện gì đang xảy ra.
*
* Thời lượng: `goal.params["wait_duration"]` cho lượt này, ngược lại param `wait_duration`.
*/
class WaitRecovery final : public recovery_core::RecoveryBehavior
{
public:
WaitRecovery() = default;
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
{
return std::make_shared<WaitRecovery>();
}
recovery_core::RecoveryOutputType outputKind() const override
{
return recovery_core::RecoveryOutputType::kNone;
}
protected:
bool onConfigure(robot::NodeHandle& nh) override
{
nh.param("wait_duration", default_wait_duration_, kDefaultWaitDuration);
default_wait_duration_ = sanitizeDuration(default_wait_duration_, kDefaultWaitDuration);
return true;
}
bool onStart(const recovery_core::RecoveryGoal& goal) override
{
wait_duration_ = sanitizeDuration(goal.param("wait_duration", default_wait_duration_),
default_wait_duration_);
return true;
}
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double /*dt*/) override
{
// elapsed() do base đo bằng đồng hồ thật, nên loop chạy chậm không làm sai thời lượng đợi.
const double waited = elapsed();
if (waited >= wait_duration_)
{
return recovery_core::RecoveryResult::Succeeded()
.withProgress(1.0, 0.0)
.withMessage("wait complete");
}
return recovery_core::RecoveryResult::Running()
.withProgress(waited / wait_duration_, wait_duration_ - waited)
.withMessage("waiting");
}
private:
double sanitizeDuration(double value, double fallback) const
{
if (!std::isfinite(value) || value <= 0.0 || value > kMaxWaitDuration)
{
robot::log_warning("[recovery_core] '%s': wait_duration=%.3f s outside (0, %.0f]; using %.3f "
"s.",
name().c_str(), value, kMaxWaitDuration, fallback);
return fallback;
}
return value;
}
double default_wait_duration_ = kDefaultWaitDuration; ///< [s]
double wait_duration_ = kDefaultWaitDuration; ///< [s] mục tiêu lượt này
};
} // namespace recovery_plugins
BOOST_DLL_ALIAS(recovery_plugins::WaitRecovery::create, WaitRecovery)