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;
};