146 lines
5.0 KiB
C++
146 lines
5.0 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* recovery_core — per-cycle backup recovery plugin (goal-driven).
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
|
|
#include <recovery_core/recovery_behavior.h>
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <string>
|
|
|
|
#include <boost/dll/alias.hpp>
|
|
#include <robot/robot.h>
|
|
|
|
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.
|
|
} // 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;
|
|
|
|
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
|
|
{
|
|
return std::make_shared<BackUpRecovery>();
|
|
}
|
|
|
|
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("control_period", control_period_, kDefaultControlPeriod);
|
|
private_nh.param("require_costmap", require_costmap_, false);
|
|
|
|
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;
|
|
}
|
|
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;
|
|
}
|
|
}
|
|
|
|
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() 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) * control_period_);
|
|
|
|
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;
|
|
double control_period_ = kDefaultControlPeriod;
|
|
bool require_costmap_ = false;
|
|
|
|
double backup_distance_ = kDefaultBackupDistance;
|
|
double linear_speed_ = kDefaultLinearSpeed;
|
|
double traveled_distance_ = 0.0;
|
|
};
|
|
|
|
} // namespace recovery_plugins
|
|
|
|
BOOST_DLL_ALIAS(recovery_plugins::BackUpRecovery::create, BackUpRecovery)
|