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

temporary storage 7/9/2026 20:41

Browse Source
This commit is contained in:
duongtd
2026-07-09 20:41:35 +07:00
parent e9394434ba
commit 17088a227d
12 changed files with 82 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
plugins/back_up_recovery.cpp
View File

@@ -22,6 +22,7 @@ namespace
{
constexpr double kDefaultBackupDistance = 0.5; // m.
constexpr double kDefaultLinearSpeed = 0.1; // m/s.
constexpr double kDefaultControlPeriod = 0.1; // s per update tick.
} // namespace
/**
@@ -48,6 +49,7 @@ protected:
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)
@@ -62,6 +64,12 @@ protected:
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
@@ -89,7 +97,7 @@ protected:
.withMessage("backup start");
}
recovery_core::RecoveryResult onUpdate(double dt) override
recovery_core::RecoveryResult onUpdate() override
{
if (traveled_distance_ >= backup_distance_)
{
@@ -98,8 +106,8 @@ protected:
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);
traveled_distance_ = std::min(
backup_distance_, traveled_distance_ + std::abs(command.linear.x) * control_period_);
if (traveled_distance_ >= backup_distance_)
{
@@ -124,6 +132,7 @@ private:
double default_backup_distance_ = kDefaultBackupDistance;
double default_linear_speed_ = kDefaultLinearSpeed;
double control_period_ = kDefaultControlPeriod;
bool require_costmap_ = false;
double backup_distance_ = kDefaultBackupDistance;