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

temporary storage 7/9/2026 19:41

Browse Source
This commit is contained in:
duongtd
2026-07-09 19:41:20 +07:00
parent 915cf85cc5
commit e9394434ba
17 changed files with 658 additions and 566 deletions
Download Patch File Download Diff File Expand all files Collapse all files

207
plugins/back_up_recovery.cpp
View File

@@ -2,18 +2,16 @@
*
* Software License Agreement (BSD License)
*
* recovery_core — per-cycle backup recovery plugin.
* recovery_core — per-cycle backup recovery plugin (goal-driven).
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_config.h>
#include <algorithm>
#include <cmath>
#include <string>
#include <vector>
#include <boost/dll/alias.hpp>
#include <robot/robot.h>
@@ -22,129 +20,114 @@ namespace recovery_plugins
{
namespace
{
robot_geometry_msgs::Twist zeroTwist()
{
return robot_geometry_msgs::Twist();
}
bool validCycle(double dt)
{
return std::isfinite(dt) && dt > 0.0;
}
constexpr double kDefaultBackupDistance = 0.5; // m.
constexpr double kDefaultLinearSpeed = 0.1; // m/s.
} // namespace
/**
* @class BackUpRecovery
* @brief Lùi thẳng tới KHOẢNG ĐÍCH do caller yêu cầu ở start(goal).
*
* 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.
*/
class BackUpRecovery final : public recovery_core::RecoveryBehavior
{
public:
BackUpRecovery() = default;
void initialize(std::string name, tf3::BufferCore* tf,
std::vector<robot_geometry_msgs::PoseStamped>* global_path,
robot_costmap_2d::Costmap2DROBOT* global_costmap,
robot_costmap_2d::Costmap2DROBOT* local_costmap) override
{
if (initialized_)
{
robot::log_error("[recovery_core] BackUpRecovery '%s' initialized twice; ignoring.",
name_.c_str());
return;
}
name_ = std::move(name);
tf_ = tf;
global_path_ = global_path;
global_costmap_ = global_costmap;
local_costmap_ = local_costmap;
robot::NodeHandle private_nh("~/" + name_);
config_ = recovery_core::RecoveryConfig::fromNodeHandle(private_nh);
private_nh.param("backup_distance", backup_distance_, 0.5);
private_nh.param("linear_speed", linear_speed_, 0.1);
private_nh.param("require_costmap", require_costmap_, false);
if (!std::isfinite(backup_distance_) || backup_distance_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid backup_distance for '%s'; using 0.5 m.",
name_.c_str());
backup_distance_ = 0.5;
}
if (!std::isfinite(linear_speed_) || linear_speed_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid linear_speed for '%s'; using 0.1 m/s.",
name_.c_str());
linear_speed_ = 0.1;
}
initialized_ = true;
status_ = recovery_core::RecoveryStatus::kIdle;
}
recovery_core::RecoveryResult runBehavior() override
{
status_ = initialized_ ? recovery_core::RecoveryStatus::kRunning :
recovery_core::RecoveryStatus::kFailed;
return initialized_ ? recovery_core::RecoveryResult::Running() :
recovery_core::RecoveryResult::Failed();
}
recovery_core::RecoveryResult computeCommand(double dt) override
{
if (!initialized_ || !validCycle(dt) || (require_costmap_ && local_costmap_ == nullptr))
{
status_ = recovery_core::RecoveryStatus::kFailed;
return recovery_core::RecoveryResult::Velocity(zeroTwist(), );
}
elapsed_ += dt;
if (config_.timeout > 0.0 && elapsed_ > config_.timeout)
{
status_ = recovery_core::RecoveryStatus::kFailed;
return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
}
if (traveled_distance_ >= backup_distance_)
{
status_ = recovery_core::RecoveryStatus::kSucceeded;
return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
}
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) * dt);
if (traveled_distance_ >= backup_distance_)
{
status_ = recovery_core::RecoveryStatus::kSucceeded;
return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
}
status_ = recovery_core::RecoveryStatus::kRunning;
return recovery_core::RecoveryResult::Velocity(command, status_);
}
recovery_core::RecoveryStatus status() const override
{
return status_;
}
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
{
return std::make_shared<BackUpRecovery>();
}
private:
tf3::BufferCore* tf_ = nullptr;
std::vector<robot_geometry_msgs::PoseStamped>* global_path_ = nullptr;
robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr;
robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr;
recovery_core::RecoveryConfig config_;
bool initialized_ = false;
recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle;
protected:
void onConfigure() 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("require_costmap", require_costmap_, false);
double backup_distance_ = 0.5;
double linear_speed_ = 0.1;
if (!std::isfinite(default_backup_distance_) || default_backup_distance_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid backup_distance for '%s'; using 0.5 m.",
name_.c_str());
default_backup_distance_ = kDefaultBackupDistance;
}
if (!std::isfinite(default_linear_speed_) || default_linear_speed_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid linear_speed for '%s'; using 0.1 m/s.",
name_.c_str());
default_linear_speed_ = kDefaultLinearSpeed;
}
}
recovery_core::RecoveryResult 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)
{
return recovery_core::RecoveryResult::Failed().withMessage("backup requires local costmap");
}
backup_distance_ = (std::isfinite(goal.distance) && goal.distance > 0.0)
? goal.distance
: default_backup_distance_;
linear_speed_ = std::abs(goal.param("linear_speed", default_linear_speed_));
if (!std::isfinite(linear_speed_) || linear_speed_ <= 0.0)
{
linear_speed_ = default_linear_speed_;
}
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");
}
recovery_core::RecoveryResult onUpdate(double dt) override
{
if (traveled_distance_ >= backup_distance_)
{
return succeeded();
}
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) * dt);
if (traveled_distance_ >= backup_distance_)
{
return succeeded();
}
return recovery_core::RecoveryResult::Velocity(command,
recovery_core::RecoveryStatus::kRunning)
.withProgress(traveled_distance_ / backup_distance_,
backup_distance_ - traveled_distance_)
.withMessage("backing up");
}
private:
recovery_core::RecoveryResult succeeded()
{
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
recovery_core::RecoveryStatus::kSucceeded)
.withProgress(1.0, 0.0)
.withMessage("backup complete");
}
double default_backup_distance_ = kDefaultBackupDistance;
double default_linear_speed_ = kDefaultLinearSpeed;
bool require_costmap_ = false;
double elapsed_ = 0.0;
double backup_distance_ = kDefaultBackupDistance;
double linear_speed_ = kDefaultLinearSpeed;
double traveled_distance_ = 0.0;
};

72
plugins/clear_costmap_recovery.cpp
View File

@@ -46,24 +46,14 @@ class ClearCostmapRecovery final : public recovery_core::RecoveryBehavior
public:
ClearCostmapRecovery() = default;
void initialize(std::string name, tf3::BufferCore* tf,
std::vector<robot_geometry_msgs::PoseStamped>* global_path,
robot_costmap_2d::Costmap2DROBOT* global_costmap,
robot_costmap_2d::Costmap2DROBOT* local_costmap) override
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
{
if (initialized_)
{
robot::log_error("[recovery_core] ClearCostmapRecovery '%s' initialized twice; ignoring.",
name_.c_str());
return;
}
name_ = std::move(name);
tf_ = tf;
global_path_ = global_path;
global_costmap_ = global_costmap;
local_costmap_ = local_costmap;
return std::make_shared<ClearCostmapRecovery>();
}
protected:
void onConfigure() 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);
@@ -89,52 +79,37 @@ public:
std::vector<std::string> clearable_layers;
private_nh.param("layer_names", clearable_layers, clearable_layers_default);
clearable_layers_.insert(clearable_layers.begin(), clearable_layers.end());
initialized_ = true;
status_ = recovery_core::RecoveryStatus::kIdle;
}
recovery_core::RecoveryResult runBehavior() override
recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& /*goal*/) override
{
if (!initialized_)
{
status_ = recovery_core::RecoveryStatus::kFailed;
return recovery_core::RecoveryResult::Failed();
}
// One-shot: công việc thực hiện ở onUpdate() lần đầu.
return recovery_core::RecoveryResult::Running().withMessage("clear costmap start");
}
recovery_core::RecoveryResult onUpdate(double /*dt*/) override
{
bool ok = true;
if (affected_maps_ == "global" || affected_maps_ == "both")
{
ok = clear(global_costmap_) && ok;
if (ok && force_updating_ && global_costmap_ != nullptr)
ok = clear(ctx().global_costmap) && ok;
if (ok && force_updating_ && ctx().global_costmap != nullptr)
{
global_costmap_->updateMap();
ctx().global_costmap->updateMap();
}
}
if (affected_maps_ == "local" || affected_maps_ == "both")
{
ok = clear(local_costmap_) && ok;
if (ok && force_updating_ && local_costmap_ != nullptr)
ok = clear(ctx().local_costmap) && ok;
if (ok && force_updating_ && ctx().local_costmap != nullptr)
{
local_costmap_->updateMap();
ctx().local_costmap->updateMap();
}
}
status_ = ok ? recovery_core::RecoveryStatus::kSucceeded :
recovery_core::RecoveryStatus::kFailed;
return ok ? recovery_core::RecoveryResult::Succeeded() :
recovery_core::RecoveryResult::Failed();
}
recovery_core::RecoveryStatus status() const override
{
return status_;
}
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
{
return std::make_shared<ClearCostmapRecovery>();
return ok ? recovery_core::RecoveryResult::Succeeded().withMessage("clear costmap complete")
: recovery_core::RecoveryResult::Failed().withMessage("clear costmap failed");
}
private:
@@ -216,13 +191,6 @@ private:
costmap->getOriginY() + costmap->getSizeInMetersY());
}
tf3::BufferCore* tf_ = nullptr;
std::vector<robot_geometry_msgs::PoseStamped>* global_path_ = nullptr;
robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr;
robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr;
bool initialized_ = false;
recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle;
bool force_updating_ = false;
double reset_distance_ = 3.0;
bool invert_area_to_clear_ = false;

84
plugins/regen_path_recovery.cpp
View File

@@ -2,81 +2,59 @@
*
* Software License Agreement (BSD License)
*
* recovery_core — path output recovery plugin.
* recovery_core — path output recovery plugin (goal-driven, one-shot).
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_behavior.h>
#include <string>
#include <vector>
#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;
void initialize(std::string name, tf3::BufferCore* tf,
std::vector<robot_geometry_msgs::PoseStamped>* global_path,
robot_costmap_2d::Costmap2DROBOT* global_costmap,
robot_costmap_2d::Costmap2DROBOT* local_costmap) override
{
if (initialized_)
{
robot::log_error("[recovery_core] RegenPathRecovery '%s' initialized twice; ignoring.",
name_.c_str());
return;
}
name_ = std::move(name);
tf_ = tf;
global_path_ = global_path;
global_costmap_ = global_costmap;
local_costmap_ = local_costmap;
initialized_ = true;
status_ = recovery_core::RecoveryStatus::kIdle;
}
recovery_core::RecoveryResult runBehavior() override
{
if (!initialized_ || global_path_ == nullptr || global_path_->empty())
{
status_ = recovery_core::RecoveryStatus::kFailed;
return recovery_core::RecoveryResult::Failed();
}
robot_nav_msgs::Path path;
path.poses = *global_path_;
status_ = recovery_core::RecoveryStatus::kSucceeded;
return recovery_core::RecoveryResult::PathOut(path, status_);
}
recovery_core::RecoveryStatus status() const override
{
return status_;
}
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
{
return std::make_shared<RegenPathRecovery>();
}
private:
tf3::BufferCore* tf_ = nullptr;
std::vector<robot_geometry_msgs::PoseStamped>* global_path_ = nullptr;
robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr;
robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr;
bool initialized_ = false;
recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle;
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(double /*dt*/) 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

143
plugins/rotate_recovery.cpp
View File

@@ -2,18 +2,16 @@
*
* Software License Agreement (BSD License)
*
* recovery_core — per-cycle rotate recovery plugin.
* recovery_core — per-cycle rotate recovery plugin (goal-driven).
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_config.h>
#include <algorithm>
#include <cmath>
#include <string>
#include <vector>
#include <boost/dll/alias.hpp>
#include <robot/robot.h>
@@ -22,90 +20,76 @@ namespace recovery_plugins
{
namespace
{
robot_geometry_msgs::Twist zeroTwist()
{
return robot_geometry_msgs::Twist();
}
bool validCycle(double dt)
{
return std::isfinite(dt) && dt > 0.0;
}
constexpr double kDefaultTargetAngle = 1.57079632679; // pi/2 rad.
constexpr double kDefaultAngularSpeed = 0.4; // rad/s.
} // namespace
/**
* @class RotateRecovery
* @brief Quay tại chỗ tới GÓC ĐÍCH do caller yêu cầu ở start(goal).
*
* 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).
*/
class RotateRecovery final : public recovery_core::RecoveryBehavior
{
public:
RotateRecovery() = default;
void initialize(std::string name, tf3::BufferCore* tf,
std::vector<robot_geometry_msgs::PoseStamped>* global_path,
robot_costmap_2d::Costmap2DROBOT* global_costmap,
robot_costmap_2d::Costmap2DROBOT* local_costmap) override
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
{
if (initialized_)
{
robot::log_error("[recovery_core] RotateRecovery '%s' initialized twice; ignoring.",
name_.c_str());
return;
}
name_ = std::move(name);
tf_ = tf;
global_path_ = global_path;
global_costmap_ = global_costmap;
local_costmap_ = local_costmap;
return std::make_shared<RotateRecovery>();
}
protected:
void onConfigure() override
{
robot::NodeHandle private_nh("~/" + name_);
config_ = recovery_core::RecoveryConfig::fromNodeHandle(private_nh);
private_nh.param("target_angle", target_angle_, 1.57079632679);
private_nh.param("angular_speed", angular_speed_, 0.4);
private_nh.param("target_angle", default_target_angle_, kDefaultTargetAngle);
private_nh.param("angular_speed", default_angular_speed_, kDefaultAngularSpeed);
if (!std::isfinite(target_angle_) || std::abs(target_angle_) <= 0.0)
if (!std::isfinite(default_target_angle_) || std::abs(default_target_angle_) <= 0.0)
{
robot::log_warning("[recovery_core] Invalid target_angle for '%s'; using pi/2.",
name_.c_str());
target_angle_ = 1.57079632679;
default_target_angle_ = kDefaultTargetAngle;
}
if (!std::isfinite(angular_speed_) || angular_speed_ <= 0.0)
if (!std::isfinite(default_angular_speed_) || default_angular_speed_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid angular_speed for '%s'; using 0.4 rad/s.",
name_.c_str());
angular_speed_ = 0.4;
default_angular_speed_ = kDefaultAngularSpeed;
}
initialized_ = true;
status_ = recovery_core::RecoveryStatus::kIdle;
}
recovery_core::RecoveryResult runBehavior() override
recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override
{
status_ = initialized_ ? recovery_core::RecoveryStatus::kRunning :
recovery_core::RecoveryStatus::kFailed;
return initialized_ ? recovery_core::RecoveryResult::Running() :
recovery_core::RecoveryResult::Failed();
// 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)
{
angular_speed_ = default_angular_speed_;
}
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");
}
recovery_core::RecoveryResult computeCommand(double dt) override
recovery_core::RecoveryResult onUpdate(double dt) override
{
if (!initialized_ || !validCycle(dt))
{
status_ = recovery_core::RecoveryStatus::kFailed;
return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
}
elapsed_ += dt;
if (config_.timeout > 0.0 && elapsed_ > config_.timeout)
{
status_ = recovery_core::RecoveryStatus::kFailed;
return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
}
const double target = std::abs(target_angle_);
if (rotated_angle_ >= target)
{
status_ = recovery_core::RecoveryStatus::kSucceeded;
return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
return succeeded(target);
}
robot_geometry_msgs::Twist command;
@@ -114,36 +98,29 @@ public:
if (rotated_angle_ >= target)
{
status_ = recovery_core::RecoveryStatus::kSucceeded;
return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
return succeeded(target);
}
status_ = recovery_core::RecoveryStatus::kRunning;
return recovery_core::RecoveryResult::Velocity(command, status_);
}
recovery_core::RecoveryStatus status() const override
{
return status_;
}
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
{
return std::make_shared<RotateRecovery>();
return recovery_core::RecoveryResult::Velocity(command,
recovery_core::RecoveryStatus::kRunning)
.withProgress(rotated_angle_ / target, target - rotated_angle_)
.withMessage("rotating");
}
private:
tf3::BufferCore* tf_ = nullptr;
std::vector<robot_geometry_msgs::PoseStamped>* global_path_ = nullptr;
robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr;
robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr;
recovery_core::RecoveryConfig config_;
bool initialized_ = false;
recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle;
recovery_core::RecoveryResult succeeded(double target)
{
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
recovery_core::RecoveryStatus::kSucceeded)
.withProgress(1.0, 0.0)
.withMessage("rotate complete");
}
double target_angle_ = 1.57079632679;
double angular_speed_ = 0.4;
double elapsed_ = 0.0;
double default_target_angle_ = kDefaultTargetAngle;
double default_angular_speed_ = kDefaultAngularSpeed;
double target_angle_ = kDefaultTargetAngle;
double angular_speed_ = kDefaultAngularSpeed;
double rotated_angle_ = 0.0;
};