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

46
src/recovery_behavior.cpp
View File

@@ -1,17 +1,13 @@
/*********************************************************************
* recovery_core — phần chung (template-method) của RecoveryBehavior.
*
* configure()/start()/update()/cancel() là NON-VIRTUAL: base lo guard vòng đời, timeout, đo
* elapsed, và xử lý cancel; plugin chỉ triển khai onConfigure()/onStart()/onUpdate().
* configure()/start()/update()/cancel() là NON-VIRTUAL: base lo guard vòng đời và xử lý
* cancel; plugin chỉ triển khai onConfigure()/onStart()/onUpdate().
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_behavior.h>
#include <cmath>
#include <robot/robot.h>
namespace recovery_core
{
namespace
@@ -20,11 +16,6 @@ robot_geometry_msgs::Twist zeroTwist()
{
return robot_geometry_msgs::Twist();
}
bool validCycle(double dt)
{
return std::isfinite(dt) && dt > 0.0;
}
} // namespace
void RecoveryBehavior::configure(const std::string& name, const RecoveryContext& ctx)
@@ -38,16 +29,6 @@ void RecoveryBehavior::configure(const std::string& name, const RecoveryContext&
ctx_ = ctx;
status_ = RecoveryStatus::kIdle;
// Param chung duy nhất còn lại: timeout (s, 0 = không giới hạn).
robot::NodeHandle private_nh("~/" + name_);
private_nh.param("timeout", timeout_, 0.0);
if (!std::isfinite(timeout_) || timeout_ < 0.0)
{
robot::log_warning("[recovery_core] Invalid timeout for '%s'; using 0 (no timeout).",
name_.c_str());
timeout_ = 0.0;
}
onConfigure();
configured_ = true;
@@ -62,15 +43,16 @@ RecoveryResult RecoveryBehavior::start(const RecoveryGoal& goal)
}
goal_ = goal;
elapsed_ = 0.0;
cancel_requested_ = false;
started_ = true;
status_ = RecoveryStatus::kRunning;
return onStart(goal_);
RecoveryResult result = onStart(goal_);
status_ = result.status;
return result;
}
RecoveryResult RecoveryBehavior::update(double dt)
RecoveryResult RecoveryBehavior::update()
{
if (!configured_ || !started_)
{
@@ -91,21 +73,7 @@ RecoveryResult RecoveryBehavior::update(double dt)
.withMessage("cancelled by caller");
}
if (!validCycle(dt))
{
status_ = RecoveryStatus::kFailed;
return RecoveryResult::Velocity(zeroTwist(), status_).withMessage("invalid dt");
}
elapsed_ += dt;
if (config_.timeout > 0.0 && elapsed_ > config_.timeout)
{
status_ = RecoveryStatus::kFailed;
return RecoveryResult::Velocity(zeroTwist(), status_).withMessage("timeout");
}
RecoveryResult result = onUpdate(dt);
result.elapsed = elapsed_;
RecoveryResult result = onUpdate();
status_ = result.status;
return result;
}