87 lines
1.9 KiB
C++
87 lines
1.9 KiB
C++
/*********************************************************************
|
|
* recovery_core — phần chung (template-method) của RecoveryBehavior.
|
|
*
|
|
* 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>
|
|
|
|
namespace recovery_core
|
|
{
|
|
namespace
|
|
{
|
|
robot_geometry_msgs::Twist zeroTwist()
|
|
{
|
|
return robot_geometry_msgs::Twist();
|
|
}
|
|
} // namespace
|
|
|
|
void RecoveryBehavior::configure(const std::string& name, const RecoveryContext& ctx)
|
|
{
|
|
if (configured_)
|
|
{
|
|
return;
|
|
}
|
|
|
|
name_ = name;
|
|
ctx_ = ctx;
|
|
status_ = RecoveryStatus::kIdle;
|
|
|
|
onConfigure();
|
|
|
|
configured_ = true;
|
|
}
|
|
|
|
RecoveryResult RecoveryBehavior::start(const RecoveryGoal& goal)
|
|
{
|
|
if (!configured_)
|
|
{
|
|
status_ = RecoveryStatus::kFailed;
|
|
return RecoveryResult::Failed().withMessage("start() before configure()");
|
|
}
|
|
|
|
goal_ = goal;
|
|
cancel_requested_ = false;
|
|
started_ = true;
|
|
status_ = RecoveryStatus::kRunning;
|
|
|
|
RecoveryResult result = onStart(goal_);
|
|
status_ = result.status;
|
|
return result;
|
|
}
|
|
|
|
RecoveryResult RecoveryBehavior::update()
|
|
{
|
|
if (!configured_ || !started_)
|
|
{
|
|
status_ = RecoveryStatus::kFailed;
|
|
return RecoveryResult::Failed().withMessage("update() before start()");
|
|
}
|
|
|
|
// Đã kết thúc: giữ nguyên trạng thái, không tick thêm.
|
|
if (status_ != RecoveryStatus::kRunning)
|
|
{
|
|
return RecoveryResult::Velocity(zeroTwist(), status_);
|
|
}
|
|
|
|
if (cancel_requested_)
|
|
{
|
|
status_ = RecoveryStatus::kCancelled;
|
|
return RecoveryResult::Velocity(zeroTwist(), status_)
|
|
.withMessage("cancelled by caller");
|
|
}
|
|
|
|
RecoveryResult result = onUpdate();
|
|
status_ = result.status;
|
|
return result;
|
|
}
|
|
|
|
void RecoveryBehavior::cancel()
|
|
{
|
|
cancel_requested_ = true;
|
|
}
|
|
|
|
} // namespace recovery_core
|